├── .gitignore ├── CHANGELIST.md ├── CMakeLists.txt ├── COPYING ├── README.md ├── ccny_openni_launch ├── CMakeLists.txt ├── Makefile ├── ROS_NOBUILD ├── launch │ ├── include │ │ ├── device.launch │ │ ├── play.launch │ │ ├── proc.launch │ │ └── record.launch │ ├── openni.launch │ ├── openni_play.launch │ └── openni_record.launch └── manifest.xml ├── ccny_rgbd ├── CMakeLists.txt ├── COPYING ├── Makefile ├── cfg │ ├── .gitignore │ ├── FeatureDetector.cfg │ ├── GftDetector.cfg │ ├── OrbDetector.cfg │ ├── RGBDImageProc.cfg │ └── StarDetector.cfg ├── include │ └── ccny_rgbd │ │ ├── apps │ │ ├── feature_viewer.h │ │ ├── keyframe_mapper.h │ │ ├── rgbd_image_proc.h │ │ └── visual_odometry.h │ │ ├── nodelet │ │ └── rgbd_image_proc_nodelet.h │ │ ├── types.h │ │ └── util.h ├── launch │ ├── feature_viewer.launch │ ├── feature_viewer.vcg │ ├── visual_odometry.launch │ ├── vo+mapping.launch │ ├── vo+mapping.rviz │ └── vo+mapping.vcg ├── mainpage.dox ├── manifest.xml ├── nodelets │ └── rgbd_image_proc_nodelet.xml ├── src │ ├── apps │ │ ├── feature_viewer.cpp │ │ ├── keyframe_mapper.cpp │ │ ├── rgbd_image_proc.cpp │ │ └── visual_odometry.cpp │ ├── node │ │ ├── feature_viewer_node.cpp │ │ ├── keyframe_mapper_node.cpp │ │ ├── rgbd_image_proc_node.cpp │ │ └── visual_odometry_node.cpp │ ├── nodelet │ │ └── rgbd_image_proc_nodelet.cpp │ └── util.cpp └── srv │ ├── AddManualKeyframe.srv │ ├── GenerateGraph.srv │ ├── Load.srv │ ├── PublishKeyframe.srv │ ├── PublishKeyframes.srv │ ├── Save.srv │ └── SolveGraph.srv ├── ccny_rgbd_data ├── ROS_NOBUILD ├── calibration_asus_xtion_pro │ ├── depth.yml │ ├── extr.yml │ └── rgb.yml ├── calibration_openni_default │ ├── depth.yml │ ├── extr.yml │ └── rgb.yml └── manifest.xml ├── lib_rgbdtools ├── Makefile └── manifest.xml └── stack.xml /.gitignore: -------------------------------------------------------------------------------- 1 | qtc-gdbmacros 2 | .DS_Store 3 | *~ 4 | *.gch 5 | *.o 6 | *.bak 7 | *.pyc 8 | .player 9 | .svn 10 | doc 11 | moc*.cpp 12 | qrc*.cpp 13 | .project 14 | .cproject 15 | .settings 16 | Makefile.* 17 | Debug 18 | Release 19 | *.suo 20 | *.ncb 21 | *.user 22 | *.tmp 23 | .directory 24 | __init__.py 25 | __init__.pyc 26 | .pydevproject 27 | CMakeFiles 28 | CMakeCache.txt 29 | cmake_install.cmake 30 | build 31 | bin 32 | lib 33 | msg_gen 34 | srv_gen 35 | src/*/cfg 36 | warp.yml 37 | -------------------------------------------------------------------------------- /CHANGELIST.md: -------------------------------------------------------------------------------- 1 | ccny_rgbd changelist 2 | ======================== 3 | 4 | current 5 | ------------------------ 6 | * added new g2o interface (can use libg2o) 7 | * added camera path saving and loading in keyframe_mapper 8 | 9 | 0.2.0 (4/15/2013) 10 | ------------------------ 11 | * seperated into ROS apps (ccny_rgbd) and stand-alone library (rgbdtools) 12 | * added seperate package for calibration and misc files: ccny_rgbd_data 13 | * added rviz config file for groovy 14 | * added range threshold parameters to keyframe_mapper 15 | * unadvertised cloud publishing topic from rgbd_image_proc if param is set to false. Otherwise, advertised. 16 | * removed ICP registration class (ICPProbModel the default and only option right now) 17 | * added flags for verbose output 18 | 19 | 0.1.1 (3/1/2013) 20 | ------------------------ 21 | * some topic renames for consistency with other packges (odom -> vo) 22 | * added CHANGELIST 23 | * keyframe_mapper: lazy pointcloud building, reduces memory requirements considerably 24 | * rgbd_image_proc now supports non-VGA input 25 | * added Octomap exporting 26 | * added path publishing 27 | * added default calibration files for OpenNI devices 28 | * added supprt for 32FC1 images 29 | * bug fixes in manifests 30 | * bug fixes in linking 31 | 32 | 0.1.0 (2/12/2013) 33 | ------------------------ 34 | * Initial release 35 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of 5 | # directories (or patterns, but directories should suffice) that should 6 | # be excluded from the distro. This is not the place to put things that 7 | # should be ignored everywhere, like "build" directories; that happens in 8 | # rosbuild/rosbuild.cmake. Here should be listed packages that aren't 9 | # ready for inclusion in a distro. 10 | # 11 | # This list is combined with the list in rosbuild/rosbuild.cmake. Note 12 | # that CMake 2.6 may be required to ensure that the two lists are combined 13 | # properly. CMake 2.4 seems to have unpredictable scoping rules for such 14 | # variables. 15 | #list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) 16 | 17 | rosbuild_make_distribution(0.1.0) 18 | -------------------------------------------------------------------------------- /COPYING: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | CCNY RGB-D tools 2 | =================================== 3 | 4 | Ivan Dryanovski 5 | 6 | 7 | Copyright (C) 2013, City University of New York 8 | CCNY Robotics Lab 9 | 10 | 11 | Overview 12 | ----------------------------------- 13 | 14 | The stack contains ROS applications for visual odometry and mapping using RGB-D cameras. 15 | The applications are built on top of the [rgbdtools](https://github.com/ccny-ros-pkg/rgbdtools.git) library. 16 | 17 | This code is at an experimental stage, and licensed under the GPLv3 license. 18 | 19 | Installing 20 | ----------------------------------- 21 | 22 | ### From source ### 23 | 24 | Create a directory where you want the package downloaded (ex. `~/ros`), 25 | and make sure it's added to your`$ROS_PACKAGE_PATH`. 26 | 27 | If you don't have git installed, do so: 28 | 29 | sudo apt-get install git-core 30 | 31 | Download the stack from our repository: 32 | 33 | git clone https://github.com/ccny-ros-pkg/ccny_rgbd_tools.git 34 | 35 | Install any dependencies using rosdep. 36 | 37 | rosdep install ccny_rgbd_tools 38 | 39 | Alternatively, you can manually install the dependencies by 40 | 41 | sudo apt-get install libsuitesparse-dev 42 | 43 | Compile the stack: 44 | 45 | rosmake ccny_rgbd_tools 46 | 47 | If you get an error compiling `ccny_g2o`, it might be because of an incompatible `g2o` installation. Try removing `libg2o`: 48 | 49 | sudo apt-get remove ros-fuerte-libg2o 50 | sudo apt-get remove ros-groovy-libg2o 51 | 52 | Quick usage 53 | ----------------------------------- 54 | 55 | Connect your RGB-D camera and launch the Openni device. The openni_launch file will 56 | start the driver and the processing nodelets. 57 | 58 | roslaunch ccny_openni_launch openni.launch 59 | 60 | For faster performace, consider using `dynamic reconfigure` to change the sampling rate of 61 | the `rgbd_image_proc` nodelet. For example, setting it to to 0.5 will downsample the images by a factor of 2. 62 | 63 | Next, launch the visual odometry: 64 | 65 | roslaunch ccny_rgbd vo+mapping.launch 66 | 67 | Finally, launch rviz. 68 | 69 | rosrun rviz rviz 70 | 71 | For convenience, you can load the `ccny_rgbd/launch/rviz.cfg` file. 72 | 73 | References 74 | ----------------------------------- 75 | 76 | If you use this system in your reasearch, please cite the following paper: 77 | 78 | Ivan Dryanovski, Roberto G. Valenti, Jizhong Xiao. 79 | *Fast Visual Odometry and Mapping from RGB-D Data*. 80 | 2013 International Conference on Robotics and Automation (ICRA2013). 81 | 82 | More info 83 | ----------------------------------- 84 | 85 | Documentation: 86 | 87 | * ROS wiki: http://ros.org/wiki/ccny_rgbd_tools 88 | * API: http://ros.org/doc/fuerte/api/ccny_rgbd/html/ 89 | 90 | Videos: 91 | * Visual odometry & 3D mapping: http://youtu.be/YE9eKgek5pI 92 | * Feature viewer: http://youtu.be/kNkrPuBu8JA 93 | -------------------------------------------------------------------------------- /ccny_openni_launch/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 | -------------------------------------------------------------------------------- /ccny_openni_launch/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /ccny_openni_launch/ROS_NOBUILD: -------------------------------------------------------------------------------- 1 | created by rosmake to mark as installed -------------------------------------------------------------------------------- /ccny_openni_launch/launch/include/device.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | # modes: 10 | # 2 (640x480 VGA 30Hz) 11 | # 5 (320x240 QVGA 30Hz) 12 | # 8 (160x120 QQVGA 30Hz) 13 | 14 | 15 | 16 | 17 | 18 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /ccny_openni_launch/launch/include/play.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | -------------------------------------------------------------------------------- /ccny_openni_launch/launch/include/proc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 37 | 38 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /ccny_openni_launch/launch/include/record.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 12 | 13 | -------------------------------------------------------------------------------- /ccny_openni_launch/launch/openni.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | # to display rgbd_image_proc messages 11 | 12 | 13 | # modes: 14 | # 2 (640x480 VGA 30Hz) 15 | # 5 (320x240 QVGA 30Hz) 16 | # 8 (160x120 QQVGA 30Hz) 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /ccny_openni_launch/launch/openni_play.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | # for rgbd_image_proc messages 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /ccny_openni_launch/launch/openni_record.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | # modes: 12 | # 2 (640x480 VGA 30Hz) 13 | # 5 (320x240 QVGA 30Hz) 14 | # 8 (160x120 QQVGA 30Hz) 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /ccny_openni_launch/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Helper launch files for OpenNI devices, including data processing, recording, and playback. 5 | 6 | 7 | Ivan Dryanovski 8 | GPL 9 | 10 | http://ros.org/wiki/ccny_openni_launch 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /ccny_rgbd/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(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 15 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 16 | 17 | include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake) 18 | 19 | # Generate services 20 | rosbuild_gensrv() 21 | 22 | # Generate dynamic parameters 23 | rosbuild_find_ros_package(dynamic_reconfigure) 24 | include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) 25 | gencfg() 26 | 27 | ################################################################ 28 | # Dependencies 29 | ################################################################ 30 | 31 | # OpenCV 32 | FIND_PACKAGE( OpenCV REQUIRED ) 33 | include_directories(${OpenCV_INCLUDE_DIRS}) 34 | link_directories(${OpenCV_LIBRARY_DIRS}) 35 | link_libraries(${OpenCV_LIBRARIES}) 36 | 37 | # Octomap 38 | find_package(octomap REQUIRED) 39 | include_directories(${OCTOMAP_INCLUDE_DIRS}) 40 | link_directories(${OCTOMAP_LIBRARY_DIRS}) 41 | link_libraries(${OCTOMAP_LIBRARIES}) 42 | 43 | # PCL 44 | find_package(PCL REQUIRED) 45 | include_directories(${PCL_INCLUDE_DIRS}) 46 | link_directories(${PCL_LIBRARY_DIRS}) 47 | link_libraries(${PCL_LIBRARIES}) 48 | 49 | #g2o 50 | set(G2O_LIBRARIES 51 | g2o_core 52 | g2o_stuff 53 | g2o_solver_cholmod 54 | g2o_solver_csparse 55 | g2o_types_slam3d 56 | cholmod) 57 | 58 | # boost 59 | rosbuild_add_boost_directories() 60 | 61 | ################################################################ 62 | # Build visual odometry application 63 | ################################################################ 64 | 65 | rosbuild_add_executable(visual_odometry_node 66 | src/node/visual_odometry_node.cpp 67 | src/apps/visual_odometry.cpp 68 | src/util.cpp) 69 | 70 | target_link_libraries (visual_odometry_node 71 | rgbdtools 72 | boost_signals 73 | boost_system 74 | boost_filesystem 75 | ${OpenCV_LIBRARIES}) 76 | 77 | ################################################################ 78 | # Build keyframe mapper application 79 | ################################################################ 80 | 81 | rosbuild_add_executable(keyframe_mapper_node 82 | src/node/keyframe_mapper_node.cpp 83 | src/apps/keyframe_mapper.cpp 84 | src/util.cpp) 85 | 86 | target_link_libraries (keyframe_mapper_node 87 | rgbdtools 88 | boost_signals 89 | boost_system 90 | boost_filesystem 91 | boost_regex 92 | ${OpenCV_LIBRARIES} 93 | ${G2O_LIBRARIES}) 94 | 95 | ################################################################ 96 | # Build feature viewer application 97 | ################################################################ 98 | 99 | rosbuild_add_executable(feature_viewer_node 100 | src/node/feature_viewer_node.cpp 101 | src/apps/feature_viewer.cpp 102 | src/util.cpp) 103 | 104 | target_link_libraries (feature_viewer_node 105 | rgbdtools 106 | boost_signals 107 | boost_system 108 | boost_filesystem 109 | ${PCL_LIBRARIES} 110 | ${OpenCV_LIBRARIES}) 111 | 112 | ################################################################ 113 | # Build image proc application 114 | ################################################################ 115 | 116 | rosbuild_add_library(rgbd_image_proc_app 117 | src/apps/rgbd_image_proc.cpp 118 | src/util.cpp) 119 | 120 | target_link_libraries (rgbd_image_proc_app 121 | rgbdtools 122 | boost_signals 123 | boost_system 124 | boost_filesystem 125 | ${OpenCV_LIBRARIES}) 126 | 127 | rosbuild_add_executable(rgbd_image_proc_node src/node/rgbd_image_proc_node.cpp) 128 | rosbuild_add_library(rgbd_image_proc_nodelet src/nodelet/rgbd_image_proc_nodelet.cpp) 129 | 130 | target_link_libraries(rgbd_image_proc_node rgbd_image_proc_app) 131 | target_link_libraries(rgbd_image_proc_nodelet rgbd_image_proc_app) 132 | 133 | -------------------------------------------------------------------------------- /ccny_rgbd/COPYING: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. -------------------------------------------------------------------------------- /ccny_rgbd/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk 2 | -------------------------------------------------------------------------------- /ccny_rgbd/cfg/.gitignore: -------------------------------------------------------------------------------- 1 | cpp 2 | *.cfgc 3 | -------------------------------------------------------------------------------- /ccny_rgbd/cfg/FeatureDetector.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # FeatureDetector dynamic reconfigure 3 | 4 | PACKAGE='ccny_rgbd' 5 | import roslib; roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | type_enum = gen.enum( 12 | [ gen.const("GFT", str_t, "GFT", "GoodFeaturesToTrack"), gen.const("STAR", str_t, "STAR", "GoodFeaturesToTrack"), gen.const("ORB", str_t, "ORB", "GoodFeaturesToTrack"), gen.const("SURF", str_t, "SURF", "GoodFeaturesToTrack"), ], 13 | "An enum to set detector type") 14 | 15 | gen.add("detector_type", str_t, 0, "Detector type", "GFT", edit_method=type_enum) 16 | 17 | gen.add("smooth", int_t, 0, "Smoothing window half-size (pixels)", 0, 0, 5) 18 | gen.add("max_range", double_t, 0, "Maximum z-depth (meters)", 5.5, 0.0, 15.0) 19 | gen.add("max_stdev", double_t, 0, "Maximum std_dev(z) (meters)", 0.03, 0.00, 0.20) 20 | 21 | gen.add("show_keypoints", bool_t, 0, "Show 2D keypoint image", False) 22 | gen.add("publish_cloud", bool_t, 0, "Publish feature point cloud", False) 23 | gen.add("publish_covariances", bool_t, 0, "Publish feature covariance markers", False) 24 | 25 | exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "FeatureDetector")) 26 | 27 | -------------------------------------------------------------------------------- /ccny_rgbd/cfg/GftDetector.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # GftDetector dynamic reconfigure 3 | 4 | PACKAGE='ccny_rgbd' 5 | import roslib; roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | gen.add("n_features", int_t, 0, "Number of feautures requested", 400, 1, 1000) 12 | gen.add("min_distance", int_t, 0, "Minimum distance between features (pixels)", 1, 0, 15) 13 | 14 | exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "GftDetector")) 15 | 16 | -------------------------------------------------------------------------------- /ccny_rgbd/cfg/OrbDetector.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # OrbDetector dynamic reconfigure 3 | 4 | PACKAGE='ccny_rgbd' 5 | import roslib; roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | gen.add("n_features", int_t, 0, "Number of feautures requested", 400, 1, 1000) 12 | gen.add("threshold", double_t, 0, "Detection threshold", 31.0, 1.0, 200.0) 13 | 14 | exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "OrbDetector")) 15 | 16 | -------------------------------------------------------------------------------- /ccny_rgbd/cfg/RGBDImageProc.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # RGBDImageProc dynamic reconfigure 3 | 4 | PACKAGE='ccny_rgbd' 5 | import roslib; roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | gen.add("scale", double_t, 0, "Resampling scale", 0.50, 0.05, 1.00) 12 | gen.add("publish_cloud", bool_t, 0, "Publish point cloud", True) 13 | 14 | exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "RGBDImageProc")) 15 | 16 | -------------------------------------------------------------------------------- /ccny_rgbd/cfg/StarDetector.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | # STARDetector dynamic reconfigure 3 | 4 | PACKAGE='ccny_rgbd' 5 | import roslib; roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | gen.add("threshold", double_t, 0, "Detection threshold", 32.0, 1.0, 100.0) 12 | gen.add("min_distance", int_t, 0, "Minimum distance between features (pixels)", 2, 0, 15) 13 | 14 | exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "StarDetector")) 15 | 16 | -------------------------------------------------------------------------------- /ccny_rgbd/include/ccny_rgbd/apps/feature_viewer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file feature_viewer.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef CCNY_RGBD_RGBD_FEATURE_VIEWER_H 25 | #define CCNY_RGBD_RGBD_FEATURE_VIEWER_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "ccny_rgbd/types.h" 37 | #include "ccny_rgbd/util.h" 38 | #include "ccny_rgbd/FeatureDetectorConfig.h" 39 | #include "ccny_rgbd/GftDetectorConfig.h" 40 | #include "ccny_rgbd/StarDetectorConfig.h" 41 | #include "ccny_rgbd/OrbDetectorConfig.h" 42 | 43 | namespace ccny_rgbd { 44 | 45 | /** @brief Application to test and visualize the different type of 46 | * feature detectors 47 | */ 48 | class FeatureViewer 49 | { 50 | public: 51 | 52 | /** @brief Constructor from ROS nodehandles 53 | * @param nh the public nodehandle 54 | * @param nh_private the private nodehandle 55 | */ 56 | FeatureViewer(const ros::NodeHandle& nh, 57 | const ros::NodeHandle& nh_private); 58 | 59 | /** @brief Default destructor 60 | */ 61 | virtual ~FeatureViewer(); 62 | 63 | private: 64 | 65 | // **** ROS-related 66 | 67 | ros::NodeHandle nh_; ///< the public nodehandle 68 | ros::NodeHandle nh_private_; ///< the private nodehandle 69 | 70 | ros::Publisher cloud_publisher_; ///< publisher for feature point cloud 71 | ros::Publisher covariances_publisher_; ///< publisher for feature covariances 72 | 73 | FeatureDetectorConfigServer config_server_; ///< ROS dynamic reconfigure server 74 | 75 | GftDetectorConfigServerPtr gft_config_server_; ///< ROS dynamic reconfigure server for GFT params 76 | StarDetectorConfigServerPtr star_config_server_; ///< ROS dynamic reconfigure server for STAR params 77 | OrbDetectorConfigServerPtr orb_config_server_; ///< ROS dynamic reconfigure server for ORB params 78 | 79 | /** @brief Image transport for RGB message subscription */ 80 | boost::shared_ptr rgb_it_; 81 | 82 | /** @brief Image transport for depth message subscription */ 83 | boost::shared_ptr depth_it_; 84 | 85 | /** @brief Callback syncronizer */ 86 | boost::shared_ptr sync_; 87 | 88 | /** @brief RGB message subscriber */ 89 | ImageSubFilter sub_rgb_; 90 | 91 | /** @brief Depth message subscriber */ 92 | ImageSubFilter sub_depth_; 93 | 94 | /** @brief Camera info message subscriber */ 95 | CameraInfoSubFilter sub_info_; 96 | 97 | // **** parameters 98 | 99 | /** @brief Feature detector type parameter 100 | * 101 | * Possible values: 102 | * - GFT (default) 103 | * - STAR 104 | * - ORB 105 | */ 106 | std::string detector_type_; 107 | 108 | int queue_size_; ///< Subscription queue size 109 | 110 | /** @brief If true, show an OpenCV window with the features 111 | * 112 | * Note: this might slightly decrease performance 113 | */ 114 | bool show_keypoints_; 115 | 116 | /** @brief If true, publish the pcl feature cloud 117 | * 118 | * Note: this might slightly decrease performance 119 | */ 120 | bool publish_cloud_; 121 | 122 | /** @brief If true, publish the covariance markers 123 | * 124 | * Note: this might decrease performance 125 | */ 126 | bool publish_covariances_; 127 | 128 | // **** variables 129 | 130 | boost::mutex mutex_; ///< state mutex 131 | int frame_count_; ///< RGBD frame counter 132 | 133 | rgbdtools::FeatureDetectorPtr feature_detector_; ///< The feature detector object 134 | 135 | // **** private functions 136 | 137 | /** @brief Main callback for RGB, Depth, and CameraInfo messages 138 | * 139 | * @param rgb_msg RGB message (8UC3) 140 | * @param depth_msg Depth message (16UC1, in mm) 141 | * @param info_msg CameraInfo message, applies to both RGB and depth images 142 | */ 143 | void RGBDCallback(const ImageMsg::ConstPtr& rgb_msg, 144 | const ImageMsg::ConstPtr& depth_msg, 145 | const CameraInfoMsg::ConstPtr& info_msg); 146 | 147 | /** @brief Initializes all the parameters from the ROS param server 148 | */ 149 | void initParams(); 150 | 151 | /** @brief Re-instantiates the feature detector based on the detector type parameter 152 | */ 153 | void resetDetector(); 154 | 155 | /** @brief Publish the feature point cloud 156 | * 157 | * Note: this might decrease performance 158 | */ 159 | void publishFeatureCloud(rgbdtools::RGBDFrame& frame); 160 | 161 | /** @brief Publish the feature covariance markers 162 | * 163 | * Note: this might decrease performance 164 | */ 165 | void publishFeatureCovariances(rgbdtools::RGBDFrame& frame); 166 | 167 | /** @brief Show the image with keypoints 168 | * 169 | * Note: this might decrease performance 170 | */ 171 | void showKeypointImage(rgbdtools::RGBDFrame& frame); 172 | 173 | /** @brief ROS dynamic reconfigure callback function 174 | */ 175 | void reconfigCallback(FeatureDetectorConfig& config, uint32_t level); 176 | 177 | /** @brief ROS dynamic reconfigure callback function for GFT 178 | */ 179 | void gftReconfigCallback(GftDetectorConfig& config, uint32_t level); 180 | 181 | /** @brief ROS dynamic reconfigure callback function for STAR 182 | */ 183 | void starReconfigCallback(StarDetectorConfig& config, uint32_t level); 184 | 185 | /** @brief ROS dynamic reconfigure callback function for ORB 186 | */ 187 | void orbReconfigCallback(OrbDetectorConfig& config, uint32_t level); 188 | }; 189 | 190 | } // namespace ccny_rgbd 191 | 192 | #endif // CCNY_RGBD_RGBD_FEATURE_VIEWER_H 193 | -------------------------------------------------------------------------------- /ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file keyframe_mapper.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef CCNY_RGBD_KEYFRAME_MAPPER_H 25 | #define CCNY_RGBD_KEYFRAME_MAPPER_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include "ccny_rgbd/types.h" 44 | #include "ccny_rgbd/util.h" 45 | #include "ccny_rgbd/GenerateGraph.h" 46 | #include "ccny_rgbd/SolveGraph.h" 47 | #include "ccny_rgbd/AddManualKeyframe.h" 48 | #include "ccny_rgbd/PublishKeyframe.h" 49 | #include "ccny_rgbd/PublishKeyframes.h" 50 | #include "ccny_rgbd/Save.h" 51 | #include "ccny_rgbd/Load.h" 52 | 53 | namespace ccny_rgbd { 54 | 55 | /** @brief Builds a 3D map from a series of RGBD keyframes. 56 | * 57 | * The KeyframeMapper app subscribes to a stream of RGBD images, as well 58 | * a transform between a fixed and a moving frame (generated, for example, 59 | * by the VisualOdometry app). The mapper stores a sequence of keyframes 60 | * containing all the necessary data to build a textured 3D point cloud map. 61 | * 62 | * The class provides an interface to perform graph-based global alignement 63 | * (in post-processing). 64 | * 65 | * Additionally, the class provides an interface to save and load keyframes 66 | * to file, so that post-processing can be done with offline data. 67 | */ 68 | class KeyframeMapper 69 | { 70 | public: 71 | 72 | /** @brief Constructor from ROS nodehandles 73 | * @param nh the public nodehandle 74 | * @param nh_private the private nodehandle 75 | */ 76 | KeyframeMapper(const ros::NodeHandle& nh, 77 | const ros::NodeHandle& nh_private); 78 | 79 | /** @brief Default destructor 80 | */ 81 | virtual ~KeyframeMapper(); 82 | 83 | /** @brief Initializes all the parameters from the ROS param server 84 | */ 85 | void initParams(); 86 | 87 | /** @brief ROS callback to publish keyframes as point clouds 88 | * 89 | * The argument should be a regular expression string matching the 90 | * index of the desired keyframes to be published. 91 | * 92 | * Note: passing a string may require escaping the quotation marks, 93 | * otherwise ROS sometimes confuses it for an integer 94 | * 95 | * Examples: 96 | * - /publish_keyframes ".*" --> publishes all keyframes 97 | * - /publish_keyframes \"[1-9]\" --> publishes keyframes 1 to 9 98 | * 99 | * The keyframe point clouds are published one by one. 100 | */ 101 | bool publishKeyframesSrvCallback( 102 | PublishKeyframes::Request& request, 103 | PublishKeyframes::Response& response); 104 | 105 | /** @brief ROS callback to publish a single keyframe as point clouds 106 | * 107 | * The argument should be an integer with the idnex of the keyframe 108 | */ 109 | bool publishKeyframeSrvCallback( 110 | PublishKeyframe::Request& request, 111 | PublishKeyframe::Response& response); 112 | 113 | /** @brief ROS callback save all the keyframes to disk 114 | * 115 | * The argument should be a string with the directory where to save 116 | * the keyframes. 117 | */ 118 | bool saveKeyframesSrvCallback( 119 | Save::Request& request, 120 | Save::Response& response); 121 | 122 | /** @brief ROS callback to create an aggregate 3D map and save it to 123 | * pcd file. 124 | * 125 | * The resolution of the map can be controlled via the \ref pcd_map_res_ 126 | * parameter. 127 | * 128 | * The argument should be the path to the .pcd file 129 | */ 130 | bool savePcdMapSrvCallback( 131 | Save::Request& request, 132 | Save::Response& response); 133 | 134 | /** @brief ROS callback to create an Octomap and save it to 135 | * file. 136 | * 137 | * The resolution of the map can be controlled via the \ref octomap_res_ 138 | * parameter. 139 | * 140 | * The argument should be the path to the .bt file 141 | */ 142 | bool saveOctomapSrvCallback( 143 | Save::Request& request, 144 | Save::Response& response); 145 | 146 | /** @brief ROS callback load keyframes from disk 147 | * 148 | * The argument should be a string with the directory pointing to 149 | * the keyframes. 150 | */ 151 | bool loadKeyframesSrvCallback( 152 | Load::Request& request, 153 | Load::Response& response); 154 | 155 | /** @brief ROS callback to manually request adding a keyframe 156 | */ 157 | bool addManualKeyframeSrvCallback( 158 | AddManualKeyframe::Request& request, 159 | AddManualKeyframe::Response& response); 160 | 161 | /** @brief ROS callback to generate the graph of keyframe 162 | * correspondences for global alignment. 163 | */ 164 | bool generateGraphSrvCallback( 165 | GenerateGraph::Request& request, 166 | GenerateGraph::Response& response); 167 | 168 | /** @brief ROS callback to perform global alignment 169 | * 170 | * Note: The generate_graph service should be called prior to invoking 171 | * this service. 172 | */ 173 | bool solveGraphSrvCallback( 174 | SolveGraph::Request& request, 175 | SolveGraph::Response& response); 176 | 177 | protected: 178 | 179 | ros::NodeHandle nh_; ///< public nodehandle 180 | ros::NodeHandle nh_private_; ///< private nodepcdhandle 181 | 182 | std::string fixed_frame_; ///< the fixed frame (usually "odom") 183 | 184 | int queue_size_; ///< Subscription queue size 185 | 186 | double max_range_; ///< Maximum threshold for range (in the z-coordinate of the camera frame) 187 | double max_stdev_; ///< Maximum threshold for range (z-coordinate) standard deviation 188 | 189 | rgbdtools::KeyframeVector keyframes_; ///< vector of RGBD Keyframes 190 | 191 | /** @brief Main callback for RGB, Depth, and CameraInfo messages 192 | * 193 | * @param depth_msg Depth message (16UC1, in mm) 194 | * @param rgb_msg RGB message (8UC3) 195 | * @param info_msg CameraInfo message, applies to both RGB and depth images 196 | */ 197 | virtual void RGBDCallback(const ImageMsg::ConstPtr& rgb_msg, 198 | const ImageMsg::ConstPtr& depth_msg, 199 | const CameraInfoMsg::ConstPtr& info_msg); 200 | 201 | private: 202 | 203 | ros::Publisher keyframes_pub_; ///< ROS publisher for the keyframe point clouds 204 | ros::Publisher poses_pub_; ///< ROS publisher for the keyframe poses 205 | ros::Publisher kf_assoc_pub_; ///< ROS publisher for the keyframe associations 206 | ros::Publisher path_pub_; ///< ROS publisher for the keyframe path 207 | 208 | /** @brief ROS service to generate the graph correpondences */ 209 | ros::ServiceServer generate_graph_service_; 210 | 211 | /** @brief ROS service to optimize the graph */ 212 | ros::ServiceServer solve_graph_service_; 213 | 214 | /** @brief ROS service to publish a single keyframe */ 215 | ros::ServiceServer pub_keyframe_service_; 216 | 217 | /** @brief ROS service to publish a subset of keyframes */ 218 | ros::ServiceServer pub_keyframes_service_; 219 | 220 | /** @brief ROS service to save all keyframes to disk */ 221 | ros::ServiceServer save_kf_service_; 222 | 223 | /** @brief ROS service to save the entire map as pcd to disk */ 224 | ros::ServiceServer save_pcd_map_service_; 225 | 226 | /** @brief ROS service to save octomap to disk */ 227 | ros::ServiceServer save_octomap_service_; 228 | 229 | /** @brief ROS service to load all keyframes from disk */ 230 | ros::ServiceServer load_kf_service_; 231 | 232 | /** @brief ROS service to add a manual keyframe */ 233 | ros::ServiceServer add_manual_keyframe_service_; 234 | 235 | tf::TransformListener tf_listener_; ///< ROS transform listener 236 | 237 | /** @brief Image transport for RGB message subscription */ 238 | boost::shared_ptr rgb_it_; 239 | 240 | /** @brief Image transport for depth message subscription */ 241 | boost::shared_ptr depth_it_; 242 | 243 | /** @brief Callback syncronizer */ 244 | boost::shared_ptr sync_; 245 | 246 | /** @brief RGB message subscriber */ 247 | ImageSubFilter sub_rgb_; 248 | 249 | /** @brief Depth message subscriber */ 250 | ImageSubFilter sub_depth_; 251 | 252 | /** @brief Camera info message subscriber */ 253 | CameraInfoSubFilter sub_info_; 254 | 255 | // params 256 | double pcd_map_res_; ///< downsampling resolution of pcd map (in meters) 257 | double octomap_res_; ///< tree resolution for octomap (in meters) 258 | double kf_dist_eps_; ///< linear distance threshold between keyframes 259 | double kf_angle_eps_; ///< angular distance threshold between keyframes 260 | bool octomap_with_color_; ///< whetehr to save Octomaps with color info 261 | double max_map_z_; ///< maximum z (in fixed frame) when exporting maps. 262 | 263 | // state vars 264 | bool manual_add_; ///< flag indicating whetehr a manual add has been requested 265 | 266 | int rgbd_frame_index_; 267 | 268 | rgbdtools::KeyframeGraphDetector graph_detector_; ///< builds graph from the keyframes 269 | rgbdtools::KeyframeGraphSolverG2O graph_solver_; ///< optimizes the graph for global alignement 270 | 271 | rgbdtools::KeyframeAssociationVector associations_; ///< keyframe associations that form the graph 272 | 273 | PathMsg path_msg_; /// < contains a vector of positions of the camera (not base) pose 274 | 275 | /** @brief processes an incoming RGBD frame with a given pose, 276 | * and determines whether a keyframe should be inserted 277 | * @param frame the incoming RGBD frame (image) 278 | * @param pose the pose of the base frame when RGBD image was taken 279 | * @retval true a keyframe was inserted 280 | * @retval false no keyframe was inserted 281 | */ 282 | bool processFrame(const rgbdtools::RGBDFrame& frame, const AffineTransform& pose); 283 | 284 | /** @brief creates a keyframe from an RGBD frame and inserts it in 285 | * the keyframe vector. 286 | * @param frame the incoming RGBD frame (image) 287 | * @param pose the pose of the base frame when RGBD image was taken 288 | */ 289 | void addKeyframe(const rgbdtools::RGBDFrame& frame, const AffineTransform& pose); 290 | 291 | /** @brief Publishes the point cloud associated with a keyframe 292 | * @param i the keyframe index 293 | */ 294 | void publishKeyframeData(int i); 295 | 296 | /** @brief Publishes the pose marker associated with a keyframe 297 | * @param i the keyframe index 298 | */ 299 | void publishKeyframePose(int i); 300 | 301 | /** @brief Publishes all the keyframe associations markers 302 | */ 303 | void publishKeyframeAssociations(); 304 | 305 | /** @brief Publishes all the keyframe pose markers 306 | */ 307 | void publishKeyframePoses(); 308 | 309 | /** @brief Publishes all the path message 310 | */ 311 | void publishPath(); 312 | 313 | /** @brief Save the full map to disk as pcd 314 | * @param path path to save the map to 315 | * @retval true save was successful 316 | * @retval false save failed. 317 | */ 318 | bool savePcdMap(const std::string& path); 319 | 320 | /** @brief Builds an pcd map from all keyframes 321 | * @param map_cloud the point cloud to be built 322 | */ 323 | void buildPcdMap(PointCloudT& map_cloud); 324 | 325 | /** @brief Save the full map to disk as octomap 326 | * @param path path to save the map to 327 | * @retval true save was successful 328 | * @retval false save failed. 329 | */ 330 | bool saveOctomap(const std::string& path); 331 | 332 | /** @brief Builds an octomap octree from all keyframes 333 | * @param tree reference to the octomap octree 334 | */ 335 | void buildOctomap(octomap::OcTree& tree); 336 | 337 | /** @brief Builds an octomap octree from all keyframes, with color 338 | * @param tree reference to the octomap octree 339 | */ 340 | void buildColorOctomap(octomap::ColorOcTree& tree); 341 | 342 | /** @brief Convert a tf pose to octomap pose 343 | * @param poseTf the tf pose 344 | * @return octomap pose 345 | */ 346 | static inline octomap::pose6d poseTfToOctomap( 347 | const tf::Pose& poseTf) 348 | { 349 | return octomap::pose6d( 350 | pointTfToOctomap(poseTf.getOrigin()), 351 | quaternionTfToOctomap(poseTf.getRotation())); 352 | } 353 | 354 | /** @brief Convert a tf point to octomap point 355 | * @param poseTf the tf point 356 | * @return octomap point 357 | */ 358 | static inline octomap::point3d pointTfToOctomap( 359 | const tf::Point& ptTf) 360 | { 361 | return octomap::point3d(ptTf.x(), ptTf.y(), ptTf.z()); 362 | } 363 | 364 | /** @brief Convert a tf quaternion to octomap quaternion 365 | * @param poseTf the tf quaternion 366 | * @return octomap quaternion 367 | */ 368 | static inline octomath::Quaternion quaternionTfToOctomap( 369 | const tf::Quaternion& qTf) 370 | { 371 | return octomath::Quaternion(qTf.w(), qTf.x(), qTf.y(), qTf.z()); 372 | } 373 | 374 | bool savePath(const std::string& filepath); 375 | bool savePathTUMFormat(const std::string& filepath); 376 | 377 | bool loadPath(const std::string& filepath); 378 | 379 | void updatePathFromKeyframePoses(); 380 | }; 381 | 382 | } // namespace ccny_rgbd 383 | 384 | #endif // CCNY_RGBD_KEYFRAME_MAPPER_H 385 | -------------------------------------------------------------------------------- /ccny_rgbd/include/ccny_rgbd/apps/rgbd_image_proc.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_image_proc.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef CCNY_RGBD_RGBD_IMAGE_PROC_H 25 | #define CCNY_RGBD_RGBD_IMAGE_PROC_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "ccny_rgbd/types.h" 36 | #include "ccny_rgbd/util.h" 37 | #include "ccny_rgbd/RGBDImageProcConfig.h" 38 | 39 | namespace ccny_rgbd { 40 | 41 | /** @brief Processes the raw output of OpenNI sensors to create 42 | * a stream of RGB-D images. 43 | * 44 | * The RGBDImageProc app subscribes to a stream of rgb and depth messages. 45 | * If performs the following operations on the images: 46 | * - Scales the images down by an arbitrary factor (see \ref scale_ param) 47 | * - Undistorts both the RGB and depth image 48 | * - Performs unwarping on the depth image based on some polynomial model 49 | * - Registers the depth image to the RGB image 50 | * 51 | * The app then publishes the resulting pair of RGB and depth images, together 52 | * with a camera info which has no distortion, and the optimal new camera matrix 53 | * for both images. 54 | */ 55 | class RGBDImageProc 56 | { 57 | typedef RGBDImageProcConfig ProcConfig; 58 | typedef dynamic_reconfigure::Server ProcConfigServer; 59 | 60 | public: 61 | 62 | /** @brief Constructor from ROS nodehandles 63 | * @param nh the public nodehandle 64 | * @param nh_private the private nodehandle 65 | */ 66 | RGBDImageProc( 67 | const ros::NodeHandle& nh, 68 | const ros::NodeHandle& nh_private); 69 | 70 | /** @brief Default destructor 71 | */ 72 | virtual ~RGBDImageProc(); 73 | 74 | /** @brief Main RGBD callback 75 | * 76 | * @param rgb_msg RGB message (8UC3) 77 | * @param depth_msg Depth message (16UC1, in mm) 78 | * @param rgb_info_msg CameraInfo message, applies to RGB image 79 | * @param depth_info_msg CameraInfo message, applies to RGB image 80 | */ 81 | void RGBDCallback(const ImageMsg::ConstPtr& rgb_msg, 82 | const ImageMsg::ConstPtr& depth_msg, 83 | const CameraInfoMsg::ConstPtr& rgb_info_msg, 84 | const CameraInfoMsg::ConstPtr& depth_info_msg); 85 | 86 | private: 87 | 88 | ros::NodeHandle nh_; ///< the public nodehandle 89 | ros::NodeHandle nh_private_; ///< the private nodehandle 90 | 91 | boost::shared_ptr sync_; ///< ROS 4-topic synchronizer 92 | 93 | ImageTransport rgb_image_transport_; ///< ROS image transport for rgb message 94 | ImageTransport depth_image_transport_; ///< ROS image transport for depth message 95 | 96 | ImageSubFilter sub_rgb_; ///< ROS subscriber for rgb message 97 | ImageSubFilter sub_depth_; ///< ROS subscriber for depth message 98 | 99 | CameraInfoSubFilter sub_rgb_info_; ///< ROS subscriber for rgb camera info 100 | CameraInfoSubFilter sub_depth_info_; ///< ROS subscriber for depth camera info 101 | 102 | ImagePublisher rgb_publisher_; ///< ROS rgb image publisher 103 | ImagePublisher depth_publisher_; ///< ROS depth image publisher 104 | ros::Publisher info_publisher_; ///< ROS camera info publisher 105 | ros::Publisher cloud_publisher_; ///< ROS PointCloud publisher 106 | 107 | ProcConfigServer config_server_; ///< ROS dynamic reconfigure server 108 | 109 | // parameters 110 | 111 | int queue_size_; ///< ROS subscriber (and publisher) queue size parameter 112 | 113 | std::string calib_path_; ///< Path to folder where calibration files are stored 114 | bool verbose_; ///< Whether to print the rectification and unwarping messages 115 | bool unwarp_; ///< Whether to perform depth unwarping based on polynomial model 116 | bool publish_cloud_; ///< Whether to calculate and publish the dense PointCloud 117 | 118 | /** @brief Downasampling scale (0, 1]. For example, 119 | * 2.0 will result in an output image half the size of the input 120 | */ 121 | double scale_; 122 | 123 | /** @brief path to extrinsic calibration yaml file, derived 124 | * from \ref calib_path_ parameter 125 | */ 126 | std::string calib_extr_filename_; 127 | 128 | /** @brief path to depth unwarp calibration yaml file, derived 129 | * from \ref calib_path_ parameter 130 | */ 131 | std::string calib_warp_filename_; 132 | 133 | // **** state variables 134 | 135 | bool initialized_; ///< whether we have initialized from the first image 136 | boost::mutex mutex_; ///< state mutex 137 | 138 | // **** calibration 139 | 140 | /** @brief Depth unwwarping mode, based on different polynomial fits 141 | * 142 | * See \ref DepthFitMode 143 | */ 144 | int fit_mode_; 145 | 146 | cv::Size size_in_; ///< Size of the incoming images. 147 | 148 | /** @brief depth unwarp polynomial coefficient matrices */ 149 | cv::Mat coeff_0_, coeff_1_, coeff_2_; 150 | 151 | /** @brief depth unwarp polynomial coefficient matrices, 152 | * after recitfication and resizing 153 | */ 154 | cv::Mat coeff_0_rect_, coeff_1_rect_, coeff_2_rect_; 155 | 156 | /** @brief extrinsic matrix between IR and RGB camera */ 157 | cv::Mat ir2rgb_; 158 | 159 | /** @brief optimal intrinsics after rectification */ 160 | cv::Mat intr_rect_rgb_, intr_rect_depth_; 161 | 162 | /** @brief RGB CameraInfo derived from optimal matrices */ 163 | CameraInfoMsg rgb_rect_info_msg_; 164 | 165 | /** @brief Depth CameraInfo derived from optimal matrices */ 166 | CameraInfoMsg depth_rect_info_msg_; 167 | 168 | /** @brief RGB rectification maps */ 169 | cv::Mat map_rgb_1_, map_rgb_2_; 170 | 171 | /** @brief Depth rectification maps */ 172 | cv::Mat map_depth_1_, map_depth_2_; 173 | 174 | /** @brief Initializes the rectification maps from CameraInfo 175 | * messages 176 | * 177 | * @param rgb_info_msg input camera info for RGB image 178 | * @param depth_info_msg input camera info for depth image 179 | */ 180 | void initMaps( 181 | const CameraInfoMsg::ConstPtr& rgb_info_msg, 182 | const CameraInfoMsg::ConstPtr& depth_info_msg); 183 | 184 | /** @brief Loads intrinsic and extrinsic calibration 185 | * info from files 186 | */ 187 | bool loadCalibration(); 188 | 189 | /** @brief Loads depth unwarp calibration info from files 190 | */ 191 | bool loadUnwarpCalibration(); 192 | 193 | /** @brief ROS dynamic reconfigure callback function 194 | */ 195 | void reconfigCallback(ProcConfig& config, uint32_t level); 196 | }; 197 | 198 | } //namespace ccny_rgbd 199 | 200 | #endif // CCNY_RGBD_RGBD_IMAGE_PROC_H 201 | -------------------------------------------------------------------------------- /ccny_rgbd/include/ccny_rgbd/apps/visual_odometry.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file visual_odometry.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef CCNY_RGBD_RGBD_VISUAL_ODOMETRY_H 25 | #define CCNY_RGBD_RGBD_VISUAL_ODOMETRY_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "ccny_rgbd/types.h" 37 | #include "ccny_rgbd/util.h" 38 | #include "ccny_rgbd/FeatureDetectorConfig.h" 39 | #include "ccny_rgbd/GftDetectorConfig.h" 40 | #include "ccny_rgbd/StarDetectorConfig.h" 41 | #include "ccny_rgbd/OrbDetectorConfig.h" 42 | 43 | namespace ccny_rgbd { 44 | 45 | /** @brief Subscribes to incoming RGBD images and outputs 46 | * the position of the moving (base) frame wrt some fixed frame. 47 | * 48 | * The class offers a selection of sparse feature detectors (GFT, ORB, STAR), 49 | * as well as a selection of registration algorithms. The default registration 50 | * method (ICPProbModel) aligns the incoming 3D sparse features against a persistent 51 | * 3D feature model, which is continuously updated using a Kalman Filer. 52 | */ 53 | class VisualOdometry 54 | { 55 | public: 56 | 57 | /** @brief Constructor from ROS nodehandles 58 | * @param nh the public nodehandle 59 | * @param nh_private the private nodehandle 60 | */ 61 | VisualOdometry(const ros::NodeHandle& nh, 62 | const ros::NodeHandle& nh_private); 63 | 64 | /** @brief Default destructor 65 | */ 66 | virtual ~VisualOdometry(); 67 | 68 | private: 69 | 70 | // **** ROS-related 71 | 72 | ros::NodeHandle nh_; ///< the public nodehandle 73 | ros::NodeHandle nh_private_; ///< the private nodehandle 74 | tf::TransformListener tf_listener_; ///< ROS transform listener 75 | tf::TransformBroadcaster tf_broadcaster_; ///< ROS transform broadcaster 76 | ros::Publisher odom_publisher_; ///< ROS Odometry publisher 77 | ros::Publisher pose_stamped_publisher_; ///< ROS pose stamped publisher 78 | ros::Publisher path_pub_; ///< ROS publisher for the VO path 79 | 80 | ros::Publisher feature_cloud_publisher_; 81 | ros::Publisher feature_cov_publisher_; 82 | ros::Publisher model_cloud_publisher_; 83 | ros::Publisher model_cov_publisher_; 84 | 85 | FILE * diagnostics_file_; ///< File for time recording statistics 86 | std::string diagnostics_file_name_; ///< File name for time recording statistics 87 | bool save_diagnostics_; ///< indicates whether to save results to file or print to screen 88 | bool verbose_; ///< indicates whether to print diagnostics to screen 89 | 90 | GftDetectorConfigServerPtr gft_config_server_; ///< ROS dynamic reconfigure server for GFT params 91 | StarDetectorConfigServerPtr star_config_server_; ///< ROS dynamic reconfigure server for STAR params 92 | OrbDetectorConfigServerPtr orb_config_server_; ///< ROS dynamic reconfigure server for ORB params 93 | 94 | /** @brief Image transport for RGB message subscription */ 95 | boost::shared_ptr rgb_it_; 96 | 97 | /** @brief Image transport for depth message subscription */ 98 | boost::shared_ptr depth_it_; 99 | 100 | /** @brief Callback syncronizer */ 101 | boost::shared_ptr sync_; 102 | 103 | /** @brief RGB message subscriber */ 104 | ImageSubFilter sub_rgb_; 105 | 106 | /** @brief Depth message subscriber */ 107 | ImageSubFilter sub_depth_; 108 | 109 | /** @brief Camera info message subscriber */ 110 | CameraInfoSubFilter sub_info_; 111 | 112 | // **** parameters 113 | 114 | std::string fixed_frame_; ///< Fixed frame parameter 115 | std::string base_frame_; ///< Moving frame parameter 116 | bool publish_tf_; ///< Parameter whether to publish a ros tf 117 | bool publish_path_; ///< Parameter whether to publish a path message 118 | bool publish_odom_; ///< Parameter whether to publish an odom message 119 | bool publish_pose_; ///< Parameter whether to publish a pose message 120 | 121 | bool publish_feature_cloud_; 122 | bool publish_feature_cov_; 123 | 124 | bool publish_model_cloud_; 125 | bool publish_model_cov_; 126 | 127 | /** @brief Feature detector type parameter 128 | * 129 | * Possible values: 130 | * - GFT (default) 131 | * - STAR 132 | * - ORB 133 | */ 134 | std::string detector_type_; 135 | 136 | /** @brief Motion estimation (registration) type parameter 137 | * 138 | * Possible values: 139 | * - ICPProbModel (default) 140 | * - ICP 141 | */ 142 | std::string reg_type_; 143 | 144 | /** @brief If true, publish the pcl feature cloud 145 | * 146 | * Note: this might slightly decrease performance 147 | */ 148 | bool publish_cloud_; 149 | 150 | int queue_size_; ///< Subscription queue size 151 | 152 | // **** variables 153 | 154 | bool initialized_; ///< Whether the init_time and b2c_ variables have been set 155 | int frame_count_; ///< RGBD frame counter 156 | ros::Time init_time_; ///< Time of first RGBD message 157 | 158 | tf::Transform b2c_; ///< Transform from the base to the camera frame, wrt base frame 159 | tf::Transform f2b_; ///< Transform from the fixed to the base frame, wrt fixed frame 160 | 161 | boost::shared_ptr feature_detector_; ///< The feature detector object 162 | 163 | rgbdtools::MotionEstimationICPProbModel motion_estimation_; ///< The motion estimation object 164 | 165 | PathMsg path_msg_; ///< contains a vector of positions of the Base frame. 166 | 167 | // **** private functions 168 | 169 | /** @brief Main callback for RGB, Depth, and CameraInfo messages 170 | * 171 | * @param rgb_msg RGB message (8UC3) 172 | * @param depth_msg Depth message (16UC1, in mm) 173 | * @param info_msg CameraInfo message, applies to both RGB and depth images 174 | */ 175 | void RGBDCallback(const ImageMsg::ConstPtr& rgb_msg, 176 | const ImageMsg::ConstPtr& depth_msg, 177 | const CameraInfoMsg::ConstPtr& info_msg); 178 | 179 | /** @brief Initializes all the parameters from the ROS param server 180 | */ 181 | void initParams(); 182 | 183 | /** @brief Re-instantiates the feature detector based on the detector type parameter 184 | */ 185 | void resetDetector(); 186 | 187 | /** @brief publishes the f2b_ (fixed-to-base) transform as a tf 188 | * @param header header of the incoming message, used to stamp things correctly 189 | */ 190 | void publishTf(const std_msgs::Header& header); 191 | 192 | /** @brief publishes the f2b_ (fixed-to-base) transform as an Odom message 193 | * \todo publish also as PoseWithCovariance 194 | * @param header header of the incoming message, used to stamp things correctly 195 | */ 196 | void publishOdom(const std_msgs::Header& header); 197 | 198 | /** @brief publishes the f2b_ (fixed-to-base) transform as an pose stamped message 199 | * @param header header of the incoming message, used to stamp things correctly 200 | */ 201 | void publishPoseStamped(const std_msgs::Header& header); 202 | 203 | /** @brief publishes the path of f2b_ (fixed-to-base) transform as an Path message 204 | * @param header header of the incoming message, used to stamp things correctly 205 | */ 206 | void publishPath(const std_msgs::Header& header); 207 | 208 | /** @brief Publish the feature point cloud 209 | * 210 | * Note: this might decrease performance 211 | */ 212 | void publishFeatureCloud(rgbdtools::RGBDFrame& frame); 213 | 214 | void publishFeatureCovariances(rgbdtools::RGBDFrame& frame); 215 | 216 | void publishModelCloud(); 217 | 218 | void publishModelCovariances(); 219 | 220 | /** @brief Caches the transform from the base frame to the camera frame 221 | * @param header header of the incoming message, used to stamp things correctly 222 | */ 223 | bool getBaseToCameraTf(const std_msgs::Header& header); 224 | 225 | /** @brief ROS dynamic reconfigure callback function for GFT 226 | */ 227 | void gftReconfigCallback(GftDetectorConfig& config, uint32_t level); 228 | 229 | /** @brief ROS dynamic reconfigure callback function for STAR 230 | */ 231 | void starReconfigCallback(StarDetectorConfig& config, uint32_t level); 232 | 233 | /** @brief ROS dynamic reconfigure callback function for ORB 234 | */ 235 | void orbReconfigCallback(OrbDetectorConfig& config, uint32_t level); 236 | 237 | /** 238 | * @brief Saves computed running times to file (or print on screen) 239 | * @return 1 if write to file was successful 240 | */ 241 | void diagnostics( 242 | int n_features, int n_valid_features, int n_model_pts, 243 | double d_frame, double d_features, double d_reg, double d_total); 244 | 245 | void configureMotionEstimation(); 246 | }; 247 | 248 | } // namespace ccny_rgbd 249 | 250 | #endif // CCNY_RGBD_RGBD_VISUAL_ODOMETRY_H 251 | -------------------------------------------------------------------------------- /ccny_rgbd/include/ccny_rgbd/nodelet/rgbd_image_proc_nodelet.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_image_proc_nodelet.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef CCNY_RGBD_RGBD_IMAGE_PROC_NODELET_H 25 | #define CCNY_RGBD_RGBD_IMAGE_PROC_NODELET_H 26 | 27 | #include 28 | #include 29 | 30 | #include "ccny_rgbd/apps/rgbd_image_proc.h" 31 | 32 | namespace ccny_rgbd { 33 | 34 | /** @brief Nodelet driver for the RGBDImageProc class. 35 | */ 36 | class RGBDImageProcNodelet : public nodelet::Nodelet 37 | { 38 | public: 39 | virtual void onInit(); 40 | 41 | private: 42 | RGBDImageProc * rgbd_image_proc_; // FIXME: change to smart pointer 43 | }; 44 | 45 | } // namespace ccny_rgbd 46 | 47 | #endif // CCNY_RGBD_RGBD_IMAGE_PROC_NODELET_H 48 | -------------------------------------------------------------------------------- /ccny_rgbd/include/ccny_rgbd/types.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file types.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef CCNY_RGBD_TYPES_H 25 | #define CCNY_RGBD_TYPES_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "ccny_rgbd/FeatureDetectorConfig.h" 43 | #include "ccny_rgbd/GftDetectorConfig.h" 44 | #include "ccny_rgbd/StarDetectorConfig.h" 45 | #include "ccny_rgbd/SurfDetectorConfig.h" 46 | #include "ccny_rgbd/OrbDetectorConfig.h" 47 | 48 | namespace ccny_rgbd { 49 | 50 | // Eigen matrix types 51 | 52 | typedef Eigen::Matrix3f Matrix3f; 53 | typedef Eigen::Vector3f Vector3f; 54 | typedef Eigen::Affine3f Pose; 55 | typedef Eigen::Affine3f AffineTransform; 56 | 57 | // Vector types 58 | 59 | typedef std::vector IntVector; 60 | typedef std::vector FloatVector; 61 | typedef std::vector BoolVector; 62 | typedef std::vector Point2fVector; 63 | typedef std::vector Point3fVector; 64 | typedef std::vector Matrix3fVector; 65 | typedef std::vector Vector3fVector; 66 | typedef std::vector KeypointVector; 67 | 68 | typedef Eigen::aligned_allocator AffineTransformAllocator; 69 | typedef std::vector AffineTransformVector; 70 | 71 | // PCL types 72 | 73 | typedef pcl::PointXYZRGB PointT; 74 | typedef pcl::PointCloud PointCloudT; 75 | 76 | typedef pcl::PointXYZ PointFeature; 77 | typedef pcl::PointCloud PointCloudFeature; 78 | 79 | typedef pcl::KdTreeFLANN KdTree; 80 | typedef pcl::registration::TransformationEstimationSVD TransformationEstimationSVD; 81 | 82 | // ROS message types 83 | 84 | typedef sensor_msgs::Image ImageMsg; 85 | typedef sensor_msgs::CameraInfo CameraInfoMsg; 86 | typedef nav_msgs::Odometry OdomMsg; 87 | typedef nav_msgs::Path PathMsg; 88 | 89 | // ROS publishers, subscribers, services, etc 90 | 91 | typedef image_geometry::PinholeCameraModel PinholeCameraModel; 92 | typedef image_transport::ImageTransport ImageTransport; 93 | typedef image_transport::Publisher ImagePublisher; 94 | typedef image_transport::SubscriberFilter ImageSubFilter; 95 | typedef message_filters::Subscriber CameraInfoSubFilter; 96 | 97 | typedef message_filters::sync_policies::ApproximateTime RGBDSyncPolicy3; 98 | typedef message_filters::sync_policies::ApproximateTime RGBDSyncPolicy4; 99 | typedef message_filters::Synchronizer RGBDSynchronizer3; 100 | typedef message_filters::Synchronizer RGBDSynchronizer4; 101 | 102 | // ROS dynamic reconfigure 103 | 104 | typedef dynamic_reconfigure::Server FeatureDetectorConfigServer; 105 | 106 | typedef dynamic_reconfigure::Server GftDetectorConfigServer; 107 | typedef boost::shared_ptr GftDetectorConfigServerPtr; 108 | 109 | typedef dynamic_reconfigure::Server StarDetectorConfigServer; 110 | typedef boost::shared_ptr StarDetectorConfigServerPtr; 111 | 112 | typedef dynamic_reconfigure::Server SurfDetectorConfigServer; 113 | typedef boost::shared_ptr SurfDetectorConfigServerPtr; 114 | 115 | typedef dynamic_reconfigure::Server OrbDetectorConfigServer; 116 | typedef boost::shared_ptr OrbDetectorConfigServerPtr; 117 | 118 | } // namespace ccny_rgbd 119 | 120 | #endif // CCNY_RGBD_TYPES_H 121 | -------------------------------------------------------------------------------- /ccny_rgbd/include/ccny_rgbd/util.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_util.h 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #ifndef CCNY_RGBD_RGBD_UTIL_H 25 | #define CCNY_RGBD_RGBD_UTIL_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | #include "ccny_rgbd/types.h" 38 | 39 | namespace ccny_rgbd { 40 | 41 | /** @brief Given a transform, calculates the linear and angular 42 | * distance between it and identity 43 | * 44 | * @param motion the incremental motion 45 | * @param dist reference to linear distance 46 | * @param angle reference to angular distance 47 | */ 48 | void getTfDifference(const tf::Transform& motion, double& dist, double& angle); 49 | 50 | /** @brief Given two transformss, calculates the linear and angular 51 | * distance between them, or the linear and angular components of 52 | * a.inv() * b 53 | * 54 | * @param a the first transform 55 | * @param b the second transform 56 | * @param dist reference to the linear distance 57 | * @param angle reference to the angular distance 58 | */ 59 | void getTfDifference( 60 | const tf::Transform& a, 61 | const tf::Transform b, 62 | double& dist, double& angle); 63 | 64 | /** @brief Given a transfom (possibly computed as a difference between two transforms) 65 | * checks if either its angular or linar component exceeds a threshold 66 | * 67 | * @param a input transform 68 | * @param dist linear distance trheshold 69 | * @param angle angular distance threshold 70 | */ 71 | bool tfGreaterThan(const tf::Transform& a, double dist, double angle); 72 | 73 | /** @brief Converts an Eigen transform to a tf::Transform 74 | * @param trans Eigen transform 75 | * @return tf version of the eigen transform 76 | */ 77 | tf::Transform tfFromEigen(Eigen::Matrix4f trans); 78 | 79 | /** @brief Converts an tf::Transform transform to an Eigen transform 80 | * @param tf the tf transform 81 | * @return the Eigen version of the transform 82 | */ 83 | Eigen::Matrix4f eigenFromTf(const tf::Transform& tf); 84 | 85 | tf::Transform tfFromEigenAffine(const AffineTransform& trans); 86 | AffineTransform eigenAffineFromTf(const tf::Transform& tf); 87 | 88 | /** @brief Decomposes a tf into an Eigen 3x3 rotation matrix 89 | * and Eigen 3x1 rotation vector 90 | * 91 | * @param tf the transform 92 | * @param R reference to the 3x3 Eigen matrix 93 | * @param t reference to the 3x1 Eigen vector 94 | */ 95 | void tfToEigenRt( 96 | const tf::Transform& tf, 97 | Matrix3f& R, 98 | Vector3f& t); 99 | 100 | /** @brief Decomposes a tf::Transform into a 3x3 OpenCV rotation matrix 101 | * and a 3x1 OpenCV translation vector 102 | * 103 | * @param transform the transform 104 | * @param R reference to the 3x3 OpenCV matrix 105 | * @param t reference to the 3x1 OpenCV vector 106 | */ 107 | void tfToOpenCVRt( 108 | const tf::Transform& transform, 109 | cv::Mat& R, 110 | cv::Mat& t); 111 | 112 | /** @brief Creates a tf transform from a 3x3 OpenCV rotation matrix 113 | * and a 3x1 OpenCV translation vector 114 | * 115 | * @param R the 3x3 OpenCV matrix 116 | * @param t the 3x1 OpenCV vector 117 | * @param transform reference to the output transform 118 | */ 119 | void openCVRtToTf( 120 | const cv::Mat& R, 121 | const cv::Mat& t, 122 | tf::Transform& transform); 123 | 124 | /** @brief Decomposes a tf::Transform into x, y, z, roll, pitch, yaw 125 | * 126 | * @param t the input transform 127 | * @param x the output x component 128 | * @param y the output y component 129 | * @param z the output z component 130 | * @param roll the output roll component 131 | * @param pitch the output pitch component 132 | * @param yaw the output yaw component 133 | */ 134 | void tfToXYZRPY( 135 | const tf::Transform& t, 136 | double& x, double& y, double& z, 137 | double& roll, double& pitch, double& yaw); 138 | 139 | /** @brief Create OpenCV matrices from a CameraInfoMsg 140 | * 141 | * @param camera_info_msg input CameraInfoMsg 142 | * @param intr output OpenCV intrinsic matrix 143 | * @param dist output OpenCV distortion vector 144 | */ 145 | void convertCameraInfoToMats( 146 | const CameraInfoMsg::ConstPtr camera_info_msg, 147 | cv::Mat& intr, 148 | cv::Mat& dist); 149 | 150 | /** @brief Create CameraInfoMsg from OpenCV matrices (assumes no 151 | * distortion) 152 | * 153 | * @param intr input OpenCV intrinsic matrix 154 | * @param camera_info_msg output CameraInfoMsg 155 | */ 156 | void convertMatToCameraInfo( 157 | const cv::Mat& intr, 158 | CameraInfoMsg& camera_info_msg); 159 | 160 | /** @brief Returns the duration, in ms, from a given time 161 | * 162 | * @param start the start time 163 | * @return duration (in ms) from start until now 164 | */ 165 | double getMsDuration(const ros::WallTime& start); 166 | 167 | void createRGBDFrameFromROSMessages( 168 | const ImageMsg::ConstPtr& rgb_msg, 169 | const ImageMsg::ConstPtr& depth_msg, 170 | const CameraInfoMsg::ConstPtr& info_msg, 171 | rgbdtools::RGBDFrame& frame); 172 | 173 | /** @brief Copies over the poses from a Eigen vector to a ROS message. 174 | * Assumes the message is already correctly resized, and preserves 175 | * the headers of each pose in the message 176 | */ 177 | void pathEigenAffineToROS( 178 | const AffineTransformVector& path, 179 | PathMsg& path_msg); 180 | 181 | /** @brief copies over the poses from a ROS message Eigen vector. 182 | * The eigen vector will be cleared and resized appropriately. 183 | */ 184 | void pathROSToEigenAffine( 185 | const PathMsg& path_msg, 186 | AffineTransformVector& path); 187 | 188 | } // namespace ccny_rgbd 189 | 190 | #endif // CCNY_RGBD_RGBD_UTIL_H 191 | -------------------------------------------------------------------------------- /ccny_rgbd/launch/feature_viewer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | # ORB, SURF, GTF, STAR 6 | 7 | 8 | 10 | 11 | #### features ##################################### 12 | 13 | # ORB, SURF, or GFT (Good features to track) 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /ccny_rgbd/launch/feature_viewer.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorB=0.215686 2 | Background\ ColorG=0 3 | Background\ ColorR=0 4 | Camera\ Config=0.165399 3.64539 2.65175 1.80575 -0.455868 0.24773 5 | Camera\ Type=rviz::OrbitViewController 6 | Covariances.Enabled=1 7 | Covariances.Marker\ Topic=/feature/covariances 8 | Covariances.Queue\ Size=100 9 | Covariances.covariances=1 10 | Features..AxisColorAutocompute\ Value\ Bounds=1 11 | Features..AxisColorAxis=2 12 | Features..AxisColorMax\ Value=10 13 | Features..AxisColorMin\ Value=-10 14 | Features..AxisColorUse\ Fixed\ Frame=1 15 | Features..FlatColorColorB=0.14902 16 | Features..FlatColorColorG=0.945098 17 | Features..FlatColorColorR=1 18 | Features..IntensityAutocompute\ Intensity\ Bounds=1 19 | Features..IntensityChannel\ Name=intensity 20 | Features..IntensityMax\ ColorB=1 21 | Features..IntensityMax\ ColorG=1 22 | Features..IntensityMax\ ColorR=1 23 | Features..IntensityMax\ Intensity=4096 24 | Features..IntensityMin\ ColorB=0 25 | Features..IntensityMin\ ColorG=0 26 | Features..IntensityMin\ ColorR=0 27 | Features..IntensityMin\ Intensity=0 28 | Features..IntensityUse\ full\ RGB\ spectrum=0 29 | Features.Alpha=1 30 | Features.Billboard\ Size=0.01 31 | Features.Color\ Transformer=FlatColor 32 | Features.Decay\ Time=0 33 | Features.Enabled=1 34 | Features.Position\ Transformer=XYZ 35 | Features.Queue\ Size=10 36 | Features.Selectable=1 37 | Features.Style=0 38 | Features.Topic=/feature/cloud 39 | Fixed\ Frame=/camera_link 40 | Property\ Grid\ Splitter=219,171 41 | Property\ Grid\ State=expanded=.Global Options;splitterratio=0.614108 42 | QMainWindow=000000ff00000000fd0000000400000000000001140000028efc0200000006fb0000001000520047004200200052006500630074000000002c000001020000000000000000fb000000100044006900730070006c006100790073010000002c000001c6000000db01000005fb0000000a0056006900650077007301000001f5000000c5000000a801000005fb0000000c00540065006c0065006f007000000002da000000aa0000000000000000fb000000040049005200000002e4000000ca0000000000000000fb0000000a0049006d006100670065020000019a00000133000002840000020400000001000002bb00000382fc0200000002fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000002c0000009c0000006801000005fb0000001200530065006c0065006300740069006f006e000000032a00000084000000680100000500000002000003de00000125fc0100000002fb0000000600520047004200000001e0000001f20000000000000000fb0000000c0049006d00610067006500320000000000000004fa000000000000000000000003000003470000003efc0100000001fc00000000000003470000000000fffffffa000000000200000002fb0000000c0049006d00610067006500330000000000ffffffff0000000000000000fb0000000800540069006d006500000003b80000003e0000003e01000005000002300000028e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 43 | RGBD\ Cloud..AxisColorAutocompute\ Value\ Bounds=1 44 | RGBD\ Cloud..AxisColorAxis=2 45 | RGBD\ Cloud..AxisColorMax\ Value=1.08259 46 | RGBD\ Cloud..AxisColorMin\ Value=-0.759638 47 | RGBD\ Cloud..AxisColorUse\ Fixed\ Frame=1 48 | RGBD\ Cloud..FlatColorColorB=1 49 | RGBD\ Cloud..FlatColorColorG=1 50 | RGBD\ Cloud..FlatColorColorR=1 51 | RGBD\ Cloud..IntensityAutocompute\ Intensity\ Bounds=1 52 | RGBD\ Cloud..IntensityChannel\ Name=x 53 | RGBD\ Cloud..IntensityMax\ ColorB=1 54 | RGBD\ Cloud..IntensityMax\ ColorG=1 55 | RGBD\ Cloud..IntensityMax\ ColorR=1 56 | RGBD\ Cloud..IntensityMax\ Intensity=4096 57 | RGBD\ Cloud..IntensityMin\ ColorB=0 58 | RGBD\ Cloud..IntensityMin\ ColorG=0 59 | RGBD\ Cloud..IntensityMin\ ColorR=0 60 | RGBD\ Cloud..IntensityMin\ Intensity=0 61 | RGBD\ Cloud..IntensityUse\ full\ RGB\ spectrum=0 62 | RGBD\ Cloud.Alpha=0.05 63 | RGBD\ Cloud.Billboard\ Size=0.01 64 | RGBD\ Cloud.Color\ Transformer=RGB8 65 | RGBD\ Cloud.Decay\ Time=0 66 | RGBD\ Cloud.Enabled=1 67 | RGBD\ Cloud.Position\ Transformer=XYZ 68 | RGBD\ Cloud.Queue\ Size=10 69 | RGBD\ Cloud.Selectable=1 70 | RGBD\ Cloud.Style=0 71 | RGBD\ Cloud.Topic=/rgbd/cloud 72 | Target\ Frame= 73 | Tool\ 2D\ Nav\ GoalTopic=/move_base_simple/goal 74 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 75 | [Display0] 76 | ClassName=rviz::PointCloud2Display 77 | Name=RGBD Cloud 78 | [Display1] 79 | ClassName=rviz::PointCloud2Display 80 | Name=Features 81 | [Display2] 82 | ClassName=rviz::MarkerDisplay 83 | Name=Covariances 84 | [Window] 85 | Height=698 86 | Width=839 87 | X=-250 88 | Y=-1332 89 | -------------------------------------------------------------------------------- /ccny_rgbd/launch/visual_odometry.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | # ORB, SURF, GTF, STAR 6 | 7 | 8 | # ICPProbModel 9 | 10 | 11 | 13 | 14 | 23 | 24 | #### diagnostics ################################## 25 | 26 | 27 | 28 | #### frames and tf output ######################### 29 | 30 | 31 | 32 | 33 | 34 | #### features ##################################### 35 | 36 | # ORB, SURF, or GFT (Good features to track) 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | #### features: GFT ################################ 46 | 47 | 48 | 49 | 50 | #### features: SURF ############################### 51 | 52 | 53 | 54 | #### features: ORB ############################### 55 | 56 | 57 | 58 | 59 | #### registration ################################# 60 | 61 | 62 | 63 | 64 | #### registration: ICP Prob Model ################# 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /ccny_rgbd/launch/vo+mapping.launch: -------------------------------------------------------------------------------- 1 | 3 | 4 | 5 | 6 | #### VISUAL ODOMETRY #################################### 7 | 8 | # ORB, SURF, GTF, STAR 9 | 10 | 11 | # ICPProbModel, ICP 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | #### KEYFRAME MAPPING ################################### 20 | 21 | 23 | 24 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 50 | -------------------------------------------------------------------------------- /ccny_rgbd/launch/vo+mapping.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 339 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: RGBD Cloud 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Class: rviz/TF 52 | Enabled: true 53 | Frame Timeout: 15 54 | Frames: 55 | /camera_link: 56 | Value: true 57 | /camera_rgb_frame: 58 | Value: false 59 | /camera_rgb_optical_frame: 60 | Value: false 61 | /odom: 62 | Value: true 63 | All Enabled: false 64 | Marker Scale: 1 65 | Name: TF 66 | Show Arrows: true 67 | Show Axes: true 68 | Show Names: true 69 | Tree: 70 | /odom: 71 | /camera_link: 72 | /camera_rgb_frame: 73 | /camera_rgb_optical_frame: 74 | {} 75 | Update Interval: 0 76 | Value: true 77 | - Alpha: 0.2 78 | Autocompute Intensity Bounds: true 79 | Autocompute Value Bounds: 80 | Max Value: 10 81 | Min Value: -10 82 | Value: true 83 | Axis: Z 84 | Channel Name: intensity 85 | Class: rviz/PointCloud2 86 | Color: 255; 255; 255 87 | Color Transformer: RGB8 88 | Decay Time: 0 89 | Enabled: true 90 | Max Color: 255; 255; 255 91 | Max Intensity: 4096 92 | Min Color: 0; 0; 0 93 | Min Intensity: 0 94 | Name: RGBD Cloud 95 | Position Transformer: XYZ 96 | Queue Size: 10 97 | Selectable: true 98 | Size (Pixels): 3 99 | Size (m): 0.01 100 | Style: Points 101 | Topic: /rgbd/cloud 102 | Use Fixed Frame: true 103 | Use rainbow: true 104 | Value: true 105 | - Alpha: 1 106 | Autocompute Intensity Bounds: true 107 | Autocompute Value Bounds: 108 | Max Value: 10 109 | Min Value: -10 110 | Value: true 111 | Axis: Z 112 | Channel Name: intensity 113 | Class: rviz/PointCloud2 114 | Color: 255; 255; 255 115 | Color Transformer: RGB8 116 | Decay Time: 0 117 | Enabled: true 118 | Max Color: 255; 255; 255 119 | Max Intensity: 4096 120 | Min Color: 0; 0; 0 121 | Min Intensity: 0 122 | Name: Keyframes 123 | Position Transformer: XYZ 124 | Queue Size: 10 125 | Selectable: true 126 | Size (Pixels): 3 127 | Size (m): 0.01 128 | Style: Points 129 | Topic: /keyframes 130 | Use Fixed Frame: true 131 | Use rainbow: true 132 | Value: true 133 | - Alpha: 1 134 | Autocompute Intensity Bounds: true 135 | Autocompute Value Bounds: 136 | Max Value: 10 137 | Min Value: -10 138 | Value: true 139 | Axis: Z 140 | Channel Name: intensity 141 | Class: rviz/PointCloud2 142 | Color: 255; 255; 0 143 | Color Transformer: FlatColor 144 | Decay Time: 0 145 | Enabled: true 146 | Max Color: 255; 255; 255 147 | Max Intensity: 4096 148 | Min Color: 0; 0; 0 149 | Min Intensity: 0 150 | Name: Features 151 | Position Transformer: XYZ 152 | Queue Size: 10 153 | Selectable: true 154 | Size (Pixels): 3 155 | Size (m): 0.01 156 | Style: Points 157 | Topic: /feature/cloud 158 | Use Fixed Frame: true 159 | Use rainbow: true 160 | Value: true 161 | - Alpha: 1 162 | Autocompute Intensity Bounds: true 163 | Autocompute Value Bounds: 164 | Max Value: 10 165 | Min Value: -10 166 | Value: true 167 | Axis: Z 168 | Channel Name: intensity 169 | Class: rviz/PointCloud2 170 | Color: 255; 139; 141 171 | Color Transformer: FlatColor 172 | Decay Time: 0 173 | Enabled: true 174 | Max Color: 255; 255; 255 175 | Max Intensity: 4096 176 | Min Color: 0; 0; 0 177 | Min Intensity: 0 178 | Name: Model 179 | Position Transformer: XYZ 180 | Queue Size: 10 181 | Selectable: true 182 | Size (Pixels): 3 183 | Size (m): 0.01 184 | Style: Points 185 | Topic: /model/cloud 186 | Use Fixed Frame: true 187 | Use rainbow: true 188 | Value: true 189 | - Alpha: 1 190 | Buffer Length: 1 191 | Class: rviz/Path 192 | Color: 25; 255; 0 193 | Enabled: true 194 | Name: VO Path 195 | Topic: "" 196 | Value: true 197 | Enabled: true 198 | Global Options: 199 | Background Color: 0; 0; 56 200 | Fixed Frame: /odom 201 | Name: root 202 | Tools: 203 | - Class: rviz/Interact 204 | Hide Inactive Objects: true 205 | - Class: rviz/MoveCamera 206 | - Class: rviz/Select 207 | - Class: rviz/FocusCamera 208 | - Class: rviz/Measure 209 | - Class: rviz/SetInitialPose 210 | Topic: /initialpose 211 | - Class: rviz/SetGoal 212 | Topic: /move_base_simple/goal 213 | - Class: rviz/PublishPoint 214 | Single click: true 215 | Topic: /clicked_point 216 | Value: true 217 | Views: 218 | Current: 219 | Class: rviz/Orbit 220 | Distance: 7.47409 221 | Focal Point: 222 | X: 0.414791 223 | Y: -0.221346 224 | Z: 0.444704 225 | Name: Current View 226 | Near Clip Distance: 0.01 227 | Pitch: 0.615398 228 | Target Frame: 229 | Value: Orbit (rviz) 230 | Yaw: 3.5054 231 | Saved: ~ 232 | Window Geometry: 233 | Displays: 234 | collapsed: false 235 | Height: 885 236 | Hide Left Dock: false 237 | Hide Right Dock: false 238 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002adfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000001e2000000dd00fffffffb0000000a005600690065007700730100000229000000c5000000b000fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bdfc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004520000003ffc0100000002fb0000000800540069006d0065010000000000000452000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000310000002ad00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 239 | Selection: 240 | collapsed: false 241 | Time: 242 | collapsed: false 243 | Tool Properties: 244 | collapsed: false 245 | Views: 246 | collapsed: false 247 | Width: 1122 248 | X: 162 249 | Y: 18 250 | -------------------------------------------------------------------------------- /ccny_rgbd/launch/vo+mapping.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorB=0.215686 2 | Background\ ColorG=0 3 | Background\ ColorR=0 4 | Camera\ Config=1.0348 2.91406 18.6762 1.01779 -2.13659 0.257 5 | Camera\ Type=rviz::OrbitViewController 6 | Features..AxisColorAutocompute\ Value\ Bounds=1 7 | Features..AxisColorAxis=2 8 | Features..AxisColorMax\ Value=10 9 | Features..AxisColorMin\ Value=-10 10 | Features..AxisColorUse\ Fixed\ Frame=1 11 | Features..FlatColorColorB=0.14902 12 | Features..FlatColorColorG=0.945098 13 | Features..FlatColorColorR=1 14 | Features..IntensityAutocompute\ Intensity\ Bounds=1 15 | Features..IntensityChannel\ Name=intensity 16 | Features..IntensityMax\ ColorB=1 17 | Features..IntensityMax\ ColorG=1 18 | Features..IntensityMax\ ColorR=1 19 | Features..IntensityMax\ Intensity=4096 20 | Features..IntensityMin\ ColorB=0 21 | Features..IntensityMin\ ColorG=0 22 | Features..IntensityMin\ ColorR=0 23 | Features..IntensityMin\ Intensity=0 24 | Features..IntensityUse\ full\ RGB\ spectrum=0 25 | Features.Alpha=1 26 | Features.Billboard\ Size=0.01 27 | Features.Color\ Transformer=FlatColor 28 | Features.Decay\ Time=0 29 | Features.Enabled=1 30 | Features.Position\ Transformer=XYZ 31 | Features.Queue\ Size=10 32 | Features.Selectable=1 33 | Features.Style=0 34 | Features.Topic=/feature/cloud 35 | Fixed\ Frame=/odom 36 | Keyframes..AxisColorAutocompute\ Value\ Bounds=1 37 | Keyframes..AxisColorAxis=2 38 | Keyframes..AxisColorMax\ Value=10 39 | Keyframes..AxisColorMin\ Value=-10 40 | Keyframes..AxisColorUse\ Fixed\ Frame=1 41 | Keyframes..FlatColorColorB=1 42 | Keyframes..FlatColorColorG=1 43 | Keyframes..FlatColorColorR=1 44 | Keyframes..IntensityAutocompute\ Intensity\ Bounds=1 45 | Keyframes..IntensityChannel\ Name=intensity 46 | Keyframes..IntensityMax\ ColorB=1 47 | Keyframes..IntensityMax\ ColorG=1 48 | Keyframes..IntensityMax\ ColorR=1 49 | Keyframes..IntensityMax\ Intensity=4096 50 | Keyframes..IntensityMin\ ColorB=0 51 | Keyframes..IntensityMin\ ColorG=0 52 | Keyframes..IntensityMin\ ColorR=0 53 | Keyframes..IntensityMin\ Intensity=0 54 | Keyframes..IntensityUse\ full\ RGB\ spectrum=0 55 | Keyframes.Alpha=1 56 | Keyframes.Billboard\ Size=0.01 57 | Keyframes.Color\ Transformer=RGB8 58 | Keyframes.Decay\ Time=300 59 | Keyframes.Enabled=1 60 | Keyframes.Position\ Transformer=XYZ 61 | Keyframes.Queue\ Size=10 62 | Keyframes.Selectable=1 63 | Keyframes.Style=0 64 | Keyframes.Topic=/keyframes 65 | Marker.Enabled=1 66 | Marker.Marker\ Topic=/keyframe_associations 67 | Marker.Queue\ Size=100 68 | Marker.RANSAC=1 69 | Marker2.Enabled=1 70 | Marker2.Marker\ Topic=/keyframe_poses 71 | Marker2.Queue\ Size=100 72 | Model..AxisColorAutocompute\ Value\ Bounds=1 73 | Model..AxisColorAxis=2 74 | Model..AxisColorMax\ Value=10 75 | Model..AxisColorMin\ Value=-10 76 | Model..AxisColorUse\ Fixed\ Frame=1 77 | Model..FlatColorColorB=0.490196 78 | Model..FlatColorColorG=0.490196 79 | Model..FlatColorColorR=1 80 | Model..IntensityAutocompute\ Intensity\ Bounds=1 81 | Model..IntensityChannel\ Name=intensity 82 | Model..IntensityMax\ ColorB=1 83 | Model..IntensityMax\ ColorG=1 84 | Model..IntensityMax\ ColorR=1 85 | Model..IntensityMax\ Intensity=4096 86 | Model..IntensityMin\ ColorB=0 87 | Model..IntensityMin\ ColorG=0 88 | Model..IntensityMin\ ColorR=0 89 | Model..IntensityMin\ Intensity=0 90 | Model..IntensityUse\ full\ RGB\ spectrum=0 91 | Model.Alpha=1 92 | Model.Billboard\ Size=0.01 93 | Model.Color\ Transformer=FlatColor 94 | Model.Decay\ Time=0 95 | Model.Enabled=1 96 | Model.Position\ Transformer=XYZ 97 | Model.Queue\ Size=10 98 | Model.Selectable=1 99 | Model.Style=0 100 | Model.Topic=/model/cloud 101 | Path.Alpha=1 102 | Path.ColorB=0 103 | Path.ColorG=1 104 | Path.ColorR=0.1 105 | Path.Enabled=1 106 | Path.Topic=/mapper_path 107 | Property\ Grid\ Splitter=520,171 108 | Property\ Grid\ State=expanded=.Global Options,Keyframes.Enabled,Path.Enabled;splitterratio=0.614108 109 | QMainWindow=000000ff00000000fd00000004000000000000012d000003dafc0200000006fb0000001000520047004200200052006500630074000000002c000001020000000000000000fb000000100044006900730070006c006100790073010000002c000002f3000000db01000005fb0000000a005600690065007700730100000322000000e4000000a801000005fb0000000c00540065006c0065006f007000000002da000000aa0000000000000000fb000000040049005200000002e4000000ca0000000000000000fb0000000a0049006d006100670065020000019a00000133000002840000020400000001000002bb00000382fc0200000002fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000002c0000009c0000006801000005fb0000001200530065006c0065006300740069006f006e000000032a00000084000000680100000500000002000003de00000125fc0100000002fb0000000600520047004200000001e0000001f20000000000000000fb0000000c0049006d00610067006500320000000000000004fa000000000000000000000003000003470000003efc0100000001fc00000000000003470000000000fffffffa000000000200000002fb0000000c0049006d00610067006500330000000000ffffffff0000000000000000fb0000000800540069006d006500000003b80000003e0000003e0100000500000560000003da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 110 | RGBD\ Cloud..AxisColorAutocompute\ Value\ Bounds=1 111 | RGBD\ Cloud..AxisColorAxis=2 112 | RGBD\ Cloud..AxisColorMax\ Value=1.08259 113 | RGBD\ Cloud..AxisColorMin\ Value=-0.759638 114 | RGBD\ Cloud..AxisColorUse\ Fixed\ Frame=1 115 | RGBD\ Cloud..FlatColorColorB=1 116 | RGBD\ Cloud..FlatColorColorG=1 117 | RGBD\ Cloud..FlatColorColorR=1 118 | RGBD\ Cloud..IntensityAutocompute\ Intensity\ Bounds=1 119 | RGBD\ Cloud..IntensityChannel\ Name=x 120 | RGBD\ Cloud..IntensityMax\ ColorB=1 121 | RGBD\ Cloud..IntensityMax\ ColorG=1 122 | RGBD\ Cloud..IntensityMax\ ColorR=1 123 | RGBD\ Cloud..IntensityMax\ Intensity=4096 124 | RGBD\ Cloud..IntensityMin\ ColorB=0 125 | RGBD\ Cloud..IntensityMin\ ColorG=0 126 | RGBD\ Cloud..IntensityMin\ ColorR=0 127 | RGBD\ Cloud..IntensityMin\ Intensity=0 128 | RGBD\ Cloud..IntensityUse\ full\ RGB\ spectrum=0 129 | RGBD\ Cloud.Alpha=0.05 130 | RGBD\ Cloud.Billboard\ Size=0.01 131 | RGBD\ Cloud.Color\ Transformer=RGB8 132 | RGBD\ Cloud.Decay\ Time=0 133 | RGBD\ Cloud.Enabled=1 134 | RGBD\ Cloud.Position\ Transformer=XYZ 135 | RGBD\ Cloud.Queue\ Size=10 136 | RGBD\ Cloud.Selectable=1 137 | RGBD\ Cloud.Style=0 138 | RGBD\ Cloud.Topic=/rgbd/cloud 139 | TF.All\ Enabled=0 140 | TF.Enabled=1 141 | TF.Frame\ Timeout=15 142 | TF.Marker\ Scale=1 143 | TF.Show\ Arrows=0 144 | TF.Show\ Axes=1 145 | TF.Show\ Names=0 146 | TF.Update\ Interval=0 147 | Target\ Frame= 148 | Tool\ 2D\ Nav\ GoalTopic=/move_base_simple/goal 149 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 150 | [Display0] 151 | ClassName=rviz::TFDisplay 152 | Name=TF 153 | [Display1] 154 | ClassName=rviz::PointCloud2Display 155 | Name=RGBD Cloud 156 | [Display2] 157 | ClassName=rviz::PointCloud2Display 158 | Name=Features 159 | [Display3] 160 | ClassName=rviz::PointCloud2Display 161 | Name=Model 162 | [Display4] 163 | ClassName=rviz::PointCloud2Display 164 | Name=Keyframes 165 | [Display5] 166 | ClassName=rviz::PathDisplay 167 | Name=Path 168 | [Display6] 169 | ClassName=rviz::MarkerDisplay 170 | Name=Marker 171 | [Display7] 172 | ClassName=rviz::MarkerDisplay 173 | Name=Marker2 174 | [Window] 175 | Height=1050 176 | Width=1680 177 | X=1466 178 | Y=-1595 179 | -------------------------------------------------------------------------------- /ccny_rgbd/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \brief \b ccny_rgbd is a collection of tools for fast visual odometry and mapping from RGB-D data 6 | 7 | \section authors Author(s) 8 | Ivan Dryanovski 9 | 10 | \section copyright Copyright 11 | 12 | Copyright (C) 2013, City University of New York
13 | CCNY Robotics Lab
14 | http://robotics.ccny.cuny.edu 15 | 16 | \section codeapi Code API 17 | 18 | ROS Applications: 19 | - RGB-D image processing: \ref ccny_rgbd::RGBDImageProc "RGBDImageProc" 20 | - Fast visual odometry: \ref ccny_rgbd::VisualOdometry "VisualOdometry" 21 | - Keyframe-based mapping and SLAM: \ref ccny_rgbd::KeyframeMapper "KeyframeMapper" 22 | 23 | Some base classes of interest: 24 | - Base class for sparse feature detectors: \ref ccny_rgbd::FeatureDetector "FeatureDetector" 25 | - Base class for motion estimation methods: \ref ccny_rgbd::MotionEstimation "MotionEstimation" 26 | 27 | Some implementation classes of interest: 28 | - GoodFeaturesToTrack featureDetector: \ref ccny_rgbd::GftDetector "GftDetector" 29 | - KF-based motion estimation using a persistent model: \ref ccny_rgbd::MotionEstimationICPProbModel "MotionEstimationICPProbModel" 30 | - SURF + RANSAC-based graph detection: \ref ccny_rgbd::KeyframeGraphDetector "KeyframeGraphDetector" 31 | - g2o-based graph slam post-processing: \ref ccny_rgbd::KeyframeGraphSolverG2O "KeyframeGraphSolverG2O" 32 | 33 | \section reference References 34 | 35 | Ivan Dryanovski, Roberto G. Valenti, Jizhong Xiao. 36 | \b "Fast Visual Odometry and Mapping from RGB-D Data". 37 | 2013 International Conference on Robotics and Automation (ICRA2013). 38 | 39 | \section license License 40 | 41 | \b ccny_rgbd is licensed under GPLv3. 42 | 43 | */ 44 | -------------------------------------------------------------------------------- /ccny_rgbd/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Ros applications for fast visual odometry and mapping from RGB-D data, built on top of the rgbdtools library. 5 | 6 | 7 | Ivan Dryanovski 8 | GPL 9 | 10 | http://ros.org/wiki/ccny_rgbd 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /ccny_rgbd/nodelets/rgbd_image_proc_nodelet.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | RGBD Image Proc nodelet publisher. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /ccny_rgbd/src/apps/feature_viewer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file feature_viewer.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "ccny_rgbd/apps/feature_viewer.h" 25 | 26 | namespace ccny_rgbd { 27 | 28 | FeatureViewer::FeatureViewer( 29 | const ros::NodeHandle& nh, 30 | const ros::NodeHandle& nh_private): 31 | nh_(nh), 32 | nh_private_(nh_private), 33 | frame_count_(0) 34 | { 35 | ROS_INFO("Starting RGBD Feature Viewer"); 36 | 37 | // dynamic reconfigure 38 | FeatureDetectorConfigServer::CallbackType f = boost::bind( 39 | &FeatureViewer::reconfigCallback, this, _1, _2); 40 | config_server_.setCallback(f); 41 | 42 | // **** initialize ROS parameters 43 | 44 | initParams(); 45 | 46 | mutex_.lock(); 47 | resetDetector(); 48 | mutex_.unlock(); 49 | 50 | // **** publishers 51 | 52 | cloud_publisher_ = nh_.advertise( 53 | "feature/cloud", 1); 54 | covariances_publisher_ = nh_.advertise( 55 | "feature/covariances", 1); 56 | 57 | // **** subscribers 58 | 59 | ImageTransport rgb_it(nh_); 60 | ImageTransport depth_it(nh_); 61 | 62 | sub_rgb_.subscribe(rgb_it, "/rgbd/rgb", queue_size_); 63 | sub_depth_.subscribe(depth_it, "/rgbd/depth", queue_size_); 64 | sub_info_.subscribe(nh_, "/rgbd/info", queue_size_); 65 | 66 | // Synchronize inputs. 67 | sync_.reset(new RGBDSynchronizer3( 68 | RGBDSyncPolicy3(queue_size_), sub_rgb_, sub_depth_, sub_info_)); 69 | 70 | sync_->registerCallback(boost::bind(&FeatureViewer::RGBDCallback, this, _1, _2, _3)); 71 | } 72 | 73 | FeatureViewer::~FeatureViewer() 74 | { 75 | ROS_INFO("Destroying RGBD Feature Viewer"); 76 | 77 | //delete feature_detector_; 78 | } 79 | 80 | void FeatureViewer::initParams() 81 | { 82 | if (!nh_private_.getParam ("queue_size", queue_size_)) 83 | queue_size_ = 5; 84 | if (!nh_private_.getParam ("feature/detector_type", detector_type_)) 85 | detector_type_ = "GFT"; 86 | if (!nh_private_.getParam ("feature/show_keypoints", show_keypoints_)) 87 | show_keypoints_ = false; 88 | if (!nh_private_.getParam ("feature/publish_cloud", publish_cloud_)) 89 | publish_cloud_ = false; 90 | if (!nh_private_.getParam ("feature/publish_covariances", publish_covariances_)) 91 | publish_covariances_ = false; 92 | } 93 | 94 | void FeatureViewer::resetDetector() 95 | { 96 | gft_config_server_.reset(); 97 | star_config_server_.reset(); 98 | orb_config_server_.reset(); 99 | 100 | if (detector_type_ == "ORB") 101 | { 102 | ROS_INFO("Creating ORB detector"); 103 | feature_detector_.reset(new rgbdtools::OrbDetector()); 104 | orb_config_server_.reset(new 105 | OrbDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/ORB"))); 106 | 107 | // dynamic reconfigure 108 | OrbDetectorConfigServer::CallbackType f = boost::bind( 109 | &FeatureViewer::orbReconfigCallback, this, _1, _2); 110 | orb_config_server_->setCallback(f); 111 | } 112 | else if (detector_type_ == "GFT") 113 | { 114 | ROS_INFO("Creating GFT detector"); 115 | feature_detector_.reset(new rgbdtools::GftDetector()); 116 | gft_config_server_.reset(new 117 | GftDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/GFT"))); 118 | 119 | // dynamic reconfigure 120 | GftDetectorConfigServer::CallbackType f = boost::bind( 121 | &FeatureViewer::gftReconfigCallback, this, _1, _2); 122 | gft_config_server_->setCallback(f); 123 | } 124 | else if (detector_type_ == "STAR") 125 | { 126 | ROS_INFO("Creating STAR detector"); 127 | feature_detector_.reset(new rgbdtools::StarDetector()); 128 | star_config_server_.reset(new 129 | StarDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/STAR"))); 130 | 131 | // dynamic reconfigure 132 | StarDetectorConfigServer::CallbackType f = boost::bind( 133 | &FeatureViewer::starReconfigCallback, this, _1, _2); 134 | star_config_server_->setCallback(f); 135 | } 136 | else 137 | { 138 | ROS_FATAL("%s is not a valid detector type! Using GFT", detector_type_.c_str()); 139 | feature_detector_.reset(new rgbdtools::GftDetector()); 140 | gft_config_server_.reset(new 141 | GftDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/GFT"))); 142 | 143 | // dynamic reconfigure 144 | GftDetectorConfigServer::CallbackType f = boost::bind( 145 | &FeatureViewer::gftReconfigCallback, this, _1, _2); 146 | gft_config_server_->setCallback(f); 147 | } 148 | } 149 | 150 | void FeatureViewer::RGBDCallback( 151 | const ImageMsg::ConstPtr& rgb_msg, 152 | const ImageMsg::ConstPtr& depth_msg, 153 | const CameraInfoMsg::ConstPtr& info_msg) 154 | { 155 | mutex_.lock(); 156 | 157 | ros::WallTime start = ros::WallTime::now(); 158 | 159 | // create frame 160 | rgbdtools::RGBDFrame frame; 161 | createRGBDFrameFromROSMessages(rgb_msg, depth_msg, info_msg, frame); 162 | 163 | // find features 164 | feature_detector_->findFeatures(frame); 165 | 166 | ros::WallTime end = ros::WallTime::now(); 167 | 168 | // visualize 169 | 170 | if (show_keypoints_) showKeypointImage(frame); 171 | if (publish_cloud_) publishFeatureCloud(frame); 172 | if (publish_covariances_) publishFeatureCovariances(frame); 173 | 174 | // print diagnostics 175 | 176 | int n_features = frame.keypoints.size(); 177 | int n_valid_features = frame.n_valid_keypoints; 178 | 179 | double d_total = 1000.0 * (end - start).toSec(); 180 | 181 | printf("[FV %d] %s[%d][%d]: TOTAL %3.1f\n", 182 | frame_count_, detector_type_.c_str(), n_features, n_valid_features, d_total); 183 | 184 | frame_count_++; 185 | 186 | mutex_.unlock(); 187 | } 188 | 189 | void FeatureViewer::showKeypointImage(rgbdtools::RGBDFrame& frame) 190 | { 191 | cv::namedWindow("Keypoints", CV_WINDOW_NORMAL); 192 | cv::Mat kp_img(frame.rgb_img.size(), CV_8UC1); 193 | cv::drawKeypoints(frame.rgb_img, frame.keypoints, kp_img); 194 | cv::imshow("Keypoints", kp_img); 195 | cv::waitKey(1); 196 | } 197 | 198 | void FeatureViewer::publishFeatureCloud(rgbdtools::RGBDFrame& frame) 199 | { 200 | PointCloudFeature cloud; 201 | frame.constructFeaturePointCloud(cloud); 202 | cloud_publisher_.publish(cloud); 203 | } 204 | 205 | void FeatureViewer::publishFeatureCovariances(rgbdtools::RGBDFrame& frame) 206 | { 207 | // create markers 208 | visualization_msgs::Marker marker; 209 | 210 | marker.header.stamp.sec = frame.header.stamp.sec; 211 | marker.header.stamp.nsec = frame.header.stamp.nsec; 212 | marker.header.seq = frame.header.seq; 213 | marker.header.frame_id = frame.header.frame_id; 214 | 215 | marker.type = visualization_msgs::Marker::LINE_LIST; 216 | marker.color.r = 1.0; 217 | marker.color.g = 1.0; 218 | marker.color.b = 1.0; 219 | marker.color.a = 1.0; 220 | marker.scale.x = 0.0025; 221 | marker.action = visualization_msgs::Marker::ADD; 222 | marker.ns = "covariances"; 223 | marker.id = 0; 224 | marker.lifetime = ros::Duration(); 225 | 226 | for (unsigned int kp_idx = 0; kp_idx < frame.keypoints.size(); ++kp_idx) 227 | { 228 | if (!frame.kp_valid[kp_idx]) continue; 229 | 230 | const Vector3f& kp_mean = frame.kp_means[kp_idx]; 231 | const Matrix3f& kp_cov = frame.kp_covariances[kp_idx]; 232 | 233 | // transform Eigen to OpenCV matrices 234 | cv::Mat m(3, 1, CV_64F); 235 | for (int j = 0; j < 3; ++j) 236 | m.at(j, 0) = kp_mean(j, 0); 237 | 238 | cv::Mat cov(3, 3, CV_64F); 239 | for (int j = 0; j < 3; ++j) 240 | for (int i = 0; i < 3; ++i) 241 | cov.at(j, i) = kp_cov(j, i); 242 | 243 | // compute eigenvectors 244 | cv::Mat evl(1, 3, CV_64F); 245 | cv::Mat evt(3, 3, CV_64F); 246 | cv::eigen(cov, evl, evt); 247 | 248 | double mx = m.at(0,0); 249 | double my = m.at(1,0); 250 | double mz = m.at(2,0); 251 | 252 | for (int e = 0; e < 3; ++e) 253 | { 254 | geometry_msgs::Point a; 255 | geometry_msgs::Point b; 256 | 257 | double sigma = sqrt(evl.at(0,e)); 258 | double scale = sigma * 3.0; 259 | tf::Vector3 evt_tf(evt.at(e,0), 260 | evt.at(e,1), 261 | evt.at(e,2)); 262 | 263 | a.x = mx + evt_tf.getX() * scale; 264 | a.y = my + evt_tf.getY() * scale; 265 | a.z = mz + evt_tf.getZ() * scale; 266 | 267 | b.x = mx - evt_tf.getX() * scale; 268 | b.y = my - evt_tf.getY() * scale; 269 | b.z = mz - evt_tf.getZ() * scale; 270 | 271 | marker.points.push_back(a); 272 | marker.points.push_back(b); 273 | } 274 | } 275 | 276 | covariances_publisher_.publish(marker); 277 | } 278 | 279 | void FeatureViewer::reconfigCallback(FeatureDetectorConfig& config, uint32_t level) 280 | { 281 | mutex_.lock(); 282 | std::string old_detector_type = detector_type_; 283 | detector_type_ = config.detector_type; 284 | 285 | if(old_detector_type != detector_type_) 286 | resetDetector(); 287 | 288 | feature_detector_->setSmooth(config.smooth); 289 | feature_detector_->setMaxRange(config.max_range); 290 | feature_detector_->setMaxStDev(config.max_stdev); 291 | 292 | publish_cloud_ = config.publish_cloud; 293 | publish_covariances_ = config.publish_covariances; 294 | show_keypoints_ = config.show_keypoints; 295 | 296 | mutex_.unlock(); 297 | } 298 | 299 | void FeatureViewer::gftReconfigCallback(GftDetectorConfig& config, uint32_t level) 300 | { 301 | rgbdtools::GftDetectorPtr gft_detector = 302 | boost::static_pointer_cast(feature_detector_); 303 | 304 | gft_detector->setNFeatures(config.n_features); 305 | gft_detector->setMinDistance(config.min_distance); 306 | } 307 | 308 | void FeatureViewer::starReconfigCallback(StarDetectorConfig& config, uint32_t level) 309 | { 310 | rgbdtools::StarDetectorPtr star_detector = 311 | boost::static_pointer_cast(feature_detector_); 312 | 313 | star_detector->setThreshold(config.threshold); 314 | star_detector->setMinDistance(config.min_distance); 315 | } 316 | 317 | void FeatureViewer::orbReconfigCallback(OrbDetectorConfig& config, uint32_t level) 318 | { 319 | rgbdtools::OrbDetectorPtr orb_detector = 320 | boost::static_pointer_cast(feature_detector_); 321 | 322 | orb_detector->setThreshold(config.threshold); 323 | orb_detector->setNFeatures(config.n_features); 324 | } 325 | 326 | } //namespace ccny_rgbd 327 | -------------------------------------------------------------------------------- /ccny_rgbd/src/apps/keyframe_mapper.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file keyframe_mapper.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "ccny_rgbd/apps/keyframe_mapper.h" 25 | 26 | namespace ccny_rgbd { 27 | 28 | KeyframeMapper::KeyframeMapper( 29 | const ros::NodeHandle& nh, 30 | const ros::NodeHandle& nh_private): 31 | nh_(nh), 32 | nh_private_(nh_private), 33 | rgbd_frame_index_(0) 34 | { 35 | ROS_INFO("Starting RGBD Keyframe Mapper"); 36 | 37 | // **** params 38 | 39 | initParams(); 40 | 41 | // **** publishers 42 | 43 | keyframes_pub_ = nh_.advertise( 44 | "keyframes", queue_size_); 45 | poses_pub_ = nh_.advertise( 46 | "keyframe_poses", queue_size_); 47 | kf_assoc_pub_ = nh_.advertise( 48 | "keyframe_associations", queue_size_); 49 | path_pub_ = nh_.advertise( 50 | "mapper_path", queue_size_); 51 | 52 | // **** services 53 | 54 | pub_keyframe_service_ = nh_.advertiseService( 55 | "publish_keyframe", &KeyframeMapper::publishKeyframeSrvCallback, this); 56 | pub_keyframes_service_ = nh_.advertiseService( 57 | "publish_keyframes", &KeyframeMapper::publishKeyframesSrvCallback, this); 58 | save_kf_service_ = nh_.advertiseService( 59 | "save_keyframes", &KeyframeMapper::saveKeyframesSrvCallback, this); 60 | load_kf_service_ = nh_.advertiseService( 61 | "load_keyframes", &KeyframeMapper::loadKeyframesSrvCallback, this); 62 | save_pcd_map_service_ = nh_.advertiseService( 63 | "save_pcd_map", &KeyframeMapper::savePcdMapSrvCallback, this); 64 | save_octomap_service_ = nh_.advertiseService( 65 | "save_octomap", &KeyframeMapper::saveOctomapSrvCallback, this); 66 | add_manual_keyframe_service_ = nh_.advertiseService( 67 | "add_manual_keyframe", &KeyframeMapper::addManualKeyframeSrvCallback, this); 68 | generate_graph_service_ = nh_.advertiseService( 69 | "generate_graph", &KeyframeMapper::generateGraphSrvCallback, this); 70 | solve_graph_service_ = nh_.advertiseService( 71 | "solve_graph", &KeyframeMapper::solveGraphSrvCallback, this); 72 | 73 | // **** subscribers 74 | 75 | ImageTransport rgb_it(nh_); 76 | ImageTransport depth_it(nh_); 77 | 78 | sub_rgb_.subscribe(rgb_it, "/rgbd/rgb", queue_size_); 79 | sub_depth_.subscribe(depth_it, "/rgbd/depth", queue_size_); 80 | sub_info_.subscribe(nh_, "/rgbd/info", queue_size_); 81 | 82 | // Synchronize inputs. 83 | sync_.reset(new RGBDSynchronizer3( 84 | RGBDSyncPolicy3(queue_size_), sub_rgb_, sub_depth_, sub_info_)); 85 | 86 | sync_->registerCallback(boost::bind(&KeyframeMapper::RGBDCallback, this, _1, _2, _3)); 87 | } 88 | 89 | KeyframeMapper::~KeyframeMapper() 90 | { 91 | 92 | } 93 | 94 | void KeyframeMapper::initParams() 95 | { 96 | bool verbose; 97 | 98 | if (!nh_private_.getParam ("verbose", verbose)) 99 | verbose = false; 100 | if (!nh_private_.getParam ("queue_size", queue_size_)) 101 | queue_size_ = 5; 102 | if (!nh_private_.getParam ("fixed_frame", fixed_frame_)) 103 | fixed_frame_ = "/odom"; 104 | if (!nh_private_.getParam ("pcd_map_res", pcd_map_res_)) 105 | pcd_map_res_ = 0.01; 106 | if (!nh_private_.getParam ("octomap_res", octomap_res_)) 107 | octomap_res_ = 0.05; 108 | if (!nh_private_.getParam ("octomap_with_color", octomap_with_color_)) 109 | octomap_with_color_ = true; 110 | if (!nh_private_.getParam ("kf_dist_eps", kf_dist_eps_)) 111 | kf_dist_eps_ = 0.10; 112 | if (!nh_private_.getParam ("kf_angle_eps", kf_angle_eps_)) 113 | kf_angle_eps_ = 10.0 * M_PI / 180.0; 114 | if (!nh_private_.getParam ("max_range", max_range_)) 115 | max_range_ = 5.5; 116 | if (!nh_private_.getParam ("max_stdev", max_stdev_)) 117 | max_stdev_ = 0.03; 118 | if (!nh_private_.getParam ("max_map_z", max_map_z_)) 119 | max_map_z_ = std::numeric_limits::infinity(); 120 | 121 | // configure graph detection 122 | 123 | int graph_n_keypoints; 124 | int graph_n_candidates; 125 | int graph_k_nearest_neighbors; 126 | bool graph_matcher_use_desc_ratio_test = true; 127 | 128 | if (!nh_private_.getParam ("graph/n_keypoints", graph_n_keypoints)) 129 | graph_n_keypoints = 500; 130 | if (!nh_private_.getParam ("graph/n_candidates", graph_n_candidates)) 131 | graph_n_candidates = 15; 132 | if (!nh_private_.getParam ("graph/k_nearest_neighbors", graph_k_nearest_neighbors)) 133 | graph_k_nearest_neighbors = 4; 134 | 135 | graph_detector_.setNKeypoints(graph_n_keypoints); 136 | graph_detector_.setNCandidates(graph_n_candidates); 137 | graph_detector_.setKNearestNeighbors(graph_k_nearest_neighbors); 138 | graph_detector_.setMatcherUseDescRatioTest(graph_matcher_use_desc_ratio_test); 139 | 140 | graph_detector_.setSACReestimateTf(false); 141 | graph_detector_.setSACSaveResults(false); 142 | graph_detector_.setVerbose(verbose); 143 | } 144 | 145 | void KeyframeMapper::RGBDCallback( 146 | const ImageMsg::ConstPtr& rgb_msg, 147 | const ImageMsg::ConstPtr& depth_msg, 148 | const CameraInfoMsg::ConstPtr& info_msg) 149 | { 150 | tf::StampedTransform transform; 151 | 152 | const ros::Time& time = rgb_msg->header.stamp; 153 | 154 | try{ 155 | tf_listener_.waitForTransform( 156 | fixed_frame_, rgb_msg->header.frame_id, time, ros::Duration(0.1)); 157 | tf_listener_.lookupTransform( 158 | fixed_frame_, rgb_msg->header.frame_id, time, transform); 159 | } 160 | catch(...) 161 | { 162 | return; 163 | } 164 | 165 | // create a new frame and increment the counter 166 | rgbdtools::RGBDFrame frame; 167 | createRGBDFrameFromROSMessages(rgb_msg, depth_msg, info_msg, frame); 168 | frame.index = rgbd_frame_index_; 169 | rgbd_frame_index_++; 170 | 171 | bool result = processFrame(frame, eigenAffineFromTf(transform)); 172 | if (result) publishKeyframeData(keyframes_.size() - 1); 173 | 174 | publishPath(); 175 | } 176 | 177 | 178 | 179 | bool KeyframeMapper::processFrame( 180 | const rgbdtools::RGBDFrame& frame, 181 | const AffineTransform& pose) 182 | { 183 | // add the frame pose to the path vector 184 | geometry_msgs::PoseStamped frame_pose; 185 | tf::Transform frame_tf = tfFromEigenAffine(pose); 186 | tf::poseTFToMsg(frame_tf, frame_pose.pose); 187 | 188 | // update the header of the pose for the path 189 | frame_pose.header.frame_id = fixed_frame_; 190 | frame_pose.header.seq = frame.header.seq; 191 | frame_pose.header.stamp.sec = frame.header.stamp.sec; 192 | frame_pose.header.stamp.nsec = frame.header.stamp.nsec; 193 | 194 | path_msg_.poses.push_back(frame_pose); 195 | 196 | // determine if a new keyframe is needed 197 | bool result; 198 | 199 | if(keyframes_.empty() || manual_add_) 200 | { 201 | result = true; 202 | } 203 | else 204 | { 205 | double dist, angle; 206 | getTfDifference(tfFromEigenAffine(pose), 207 | tfFromEigenAffine(keyframes_.back().pose), 208 | dist, angle); 209 | 210 | if (dist > kf_dist_eps_ || angle > kf_angle_eps_) 211 | result = true; 212 | else 213 | result = false; 214 | } 215 | 216 | if (result) 217 | { 218 | addKeyframe(frame, pose); 219 | } 220 | return result; 221 | } 222 | 223 | void KeyframeMapper::addKeyframe( 224 | const rgbdtools::RGBDFrame& frame, 225 | const AffineTransform& pose) 226 | { 227 | rgbdtools::RGBDKeyframe keyframe(frame); 228 | keyframe.pose = pose; 229 | 230 | if (manual_add_) 231 | { 232 | ROS_INFO("Adding frame manually"); 233 | manual_add_ = false; 234 | keyframe.manually_added = true; 235 | } 236 | keyframes_.push_back(keyframe); 237 | } 238 | 239 | bool KeyframeMapper::publishKeyframeSrvCallback( 240 | PublishKeyframe::Request& request, 241 | PublishKeyframe::Response& response) 242 | { 243 | int kf_idx = request.id; 244 | 245 | if (kf_idx >= 0 && kf_idx < (int)keyframes_.size()) 246 | { 247 | ROS_INFO("Publishing keyframe %d", kf_idx); 248 | publishKeyframeData(kf_idx); 249 | publishKeyframePose(kf_idx); 250 | return true; 251 | } 252 | else 253 | { 254 | ROS_ERROR("Index out of range"); 255 | return false; 256 | } 257 | } 258 | 259 | bool KeyframeMapper::publishKeyframesSrvCallback( 260 | PublishKeyframes::Request& request, 261 | PublishKeyframes::Response& response) 262 | { 263 | bool found_match = false; 264 | 265 | // regex matching - try match the request string against each 266 | // keyframe index 267 | boost::regex expression(request.re); 268 | 269 | for (unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx) 270 | { 271 | std::stringstream ss; 272 | ss << kf_idx; 273 | std::string kf_idx_string = ss.str(); 274 | 275 | boost::smatch match; 276 | 277 | if(boost::regex_match(kf_idx_string, match, expression)) 278 | { 279 | found_match = true; 280 | ROS_INFO("Publishing keyframe %d", kf_idx); 281 | publishKeyframeData(kf_idx); 282 | publishKeyframePose(kf_idx); 283 | usleep(25000); 284 | } 285 | } 286 | 287 | publishPath(); 288 | 289 | return found_match; 290 | } 291 | 292 | void KeyframeMapper::publishKeyframeData(int i) 293 | { 294 | rgbdtools::RGBDKeyframe& keyframe = keyframes_[i]; 295 | 296 | // construct a cloud from the images 297 | PointCloudT cloud; 298 | keyframe.constructDensePointCloud(cloud, max_range_, max_stdev_); 299 | 300 | // cloud transformed to the fixed frame 301 | PointCloudT cloud_ff; 302 | pcl::transformPointCloud(cloud, cloud_ff, keyframe.pose); 303 | 304 | cloud_ff.header.frame_id = fixed_frame_; 305 | 306 | keyframes_pub_.publish(cloud_ff); 307 | } 308 | 309 | void KeyframeMapper::publishKeyframeAssociations() 310 | { 311 | visualization_msgs::Marker marker; 312 | marker.header.stamp = ros::Time::now(); 313 | marker.header.frame_id = fixed_frame_; 314 | marker.id = 0; 315 | marker.type = visualization_msgs::Marker::LINE_LIST; 316 | marker.action = visualization_msgs::Marker::ADD; 317 | 318 | marker.points.resize(associations_.size() * 2); 319 | 320 | marker.color.a = 1.0; 321 | 322 | for (unsigned int as_idx = 0; as_idx < associations_.size(); ++as_idx) 323 | { 324 | // set up shortcut references 325 | const rgbdtools::KeyframeAssociation& association = associations_[as_idx]; 326 | int kf_idx_a = association.kf_idx_a; 327 | int kf_idx_b = association.kf_idx_b; 328 | rgbdtools::RGBDKeyframe& keyframe_a = keyframes_[kf_idx_a]; 329 | rgbdtools::RGBDKeyframe& keyframe_b = keyframes_[kf_idx_b]; 330 | 331 | int idx_start = as_idx*2; 332 | int idx_end = as_idx*2 + 1; 333 | 334 | tf::Transform keyframe_a_pose = tfFromEigenAffine(keyframe_a.pose); 335 | tf::Transform keyframe_b_pose = tfFromEigenAffine(keyframe_b.pose); 336 | 337 | // start point for the edge 338 | marker.points[idx_start].x = keyframe_a_pose.getOrigin().getX(); 339 | marker.points[idx_start].y = keyframe_a_pose.getOrigin().getY(); 340 | marker.points[idx_start].z = keyframe_a_pose.getOrigin().getZ(); 341 | 342 | // end point for the edge 343 | marker.points[idx_end].x = keyframe_b_pose.getOrigin().getX(); 344 | marker.points[idx_end].y = keyframe_b_pose.getOrigin().getY(); 345 | marker.points[idx_end].z = keyframe_b_pose.getOrigin().getZ(); 346 | 347 | if (association.type == rgbdtools::KeyframeAssociation::VO) 348 | { 349 | marker.ns = "VO"; 350 | marker.scale.x = 0.002; 351 | 352 | marker.color.r = 0.0; 353 | marker.color.g = 1.0; 354 | marker.color.b = 0.0; 355 | } 356 | else if (association.type == rgbdtools::KeyframeAssociation::RANSAC) 357 | { 358 | marker.ns = "RANSAC"; 359 | marker.scale.x = 0.002; 360 | 361 | marker.color.r = 1.0; 362 | marker.color.g = 1.0; 363 | marker.color.b = 0.0; 364 | } 365 | 366 | kf_assoc_pub_.publish(marker); 367 | } 368 | } 369 | 370 | void KeyframeMapper::publishKeyframePoses() 371 | { 372 | for(unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx) 373 | { 374 | publishKeyframePose(kf_idx); 375 | } 376 | } 377 | 378 | void KeyframeMapper::publishKeyframePose(int i) 379 | { 380 | rgbdtools::RGBDKeyframe& keyframe = keyframes_[i]; 381 | 382 | // **** publish camera pose 383 | 384 | visualization_msgs::Marker marker; 385 | marker.header.stamp = ros::Time::now(); 386 | marker.header.frame_id = fixed_frame_; 387 | marker.ns = "keyframe_poses"; 388 | marker.id = i; 389 | marker.type = visualization_msgs::Marker::ARROW; 390 | marker.action = visualization_msgs::Marker::ADD; 391 | 392 | marker.points.resize(2); 393 | 394 | // start point for the arrow 395 | tf::Transform keyframe_pose = tfFromEigenAffine(keyframe.pose); 396 | marker.points[0].x = keyframe_pose.getOrigin().getX(); 397 | marker.points[0].y = keyframe_pose.getOrigin().getY(); 398 | marker.points[0].z = keyframe_pose.getOrigin().getZ(); 399 | 400 | // end point for the arrow 401 | tf::Transform ep; 402 | ep.setIdentity(); 403 | ep.setOrigin(tf::Vector3(0.00, 0.00, 0.12)); // z = arrow length 404 | ep = keyframe_pose * ep; 405 | 406 | marker.points[1].x = ep.getOrigin().getX(); 407 | marker.points[1].y = ep.getOrigin().getY(); 408 | marker.points[1].z = ep.getOrigin().getZ(); 409 | 410 | marker.scale.x = 0.02; // shaft radius 411 | marker.scale.y = 0.05; // head radius 412 | 413 | marker.color.a = 1.0; 414 | marker.color.r = 0.0; 415 | marker.color.g = 1.0; 416 | marker.color.b = 0.0; 417 | 418 | poses_pub_.publish(marker); 419 | 420 | // **** publish frame index text 421 | 422 | visualization_msgs::Marker marker_text; 423 | marker_text.header.stamp = ros::Time::now(); 424 | marker_text.header.frame_id = fixed_frame_; 425 | marker_text.ns = "keyframe_indexes"; 426 | marker_text.id = i; 427 | marker_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 428 | marker_text.action = visualization_msgs::Marker::ADD; 429 | 430 | tf::poseTFToMsg(keyframe_pose, marker_text.pose); 431 | 432 | marker_text.pose.position.z -= 0.05; 433 | 434 | char label[6]; 435 | sprintf(label, "%d", i); 436 | marker_text.text = label; 437 | 438 | marker_text.color.a = 1.0; 439 | marker_text.color.r = 1.0; 440 | marker_text.color.g = 1.0; 441 | marker_text.color.b = 0.0; 442 | 443 | marker_text.scale.z = 0.05; // shaft radius 444 | 445 | poses_pub_.publish(marker_text); 446 | } 447 | 448 | bool KeyframeMapper::saveKeyframesSrvCallback( 449 | Save::Request& request, 450 | Save::Response& response) 451 | { 452 | std::string filepath = request.filename; 453 | 454 | ROS_INFO("Saving keyframes..."); 455 | std::string filepath_keyframes = filepath + "/keyframes/"; 456 | bool result_kf = saveKeyframes(keyframes_, filepath_keyframes); 457 | if (result_kf) ROS_INFO("Keyframes saved to %s", filepath.c_str()); 458 | else ROS_ERROR("Keyframe saving failed!"); 459 | 460 | ROS_INFO("Saving path..."); 461 | bool result_path = savePath(filepath); 462 | savePathTUMFormat(filepath); 463 | if (result_path ) ROS_INFO("Path saved to %s", filepath.c_str()); 464 | else ROS_ERROR("Path saving failed!"); 465 | 466 | return result_kf && result_path; 467 | } 468 | 469 | bool KeyframeMapper::loadKeyframesSrvCallback( 470 | Load::Request& request, 471 | Load::Response& response) 472 | { 473 | std::string filepath = request.filename; 474 | 475 | ROS_INFO("Loading keyframes..."); 476 | std::string filepath_keyframes = filepath + "/keyframes/"; 477 | bool result_kf = loadKeyframes(keyframes_, filepath_keyframes); 478 | if (result_kf) ROS_INFO("Keyframes loaded successfully"); 479 | else ROS_ERROR("Keyframe loading failed!"); 480 | 481 | ROS_INFO("Loading path..."); 482 | bool result_path = loadPath(filepath); 483 | if (result_path) ROS_INFO("Path loaded successfully"); 484 | else ROS_ERROR("Path loading failed!"); 485 | 486 | return result_kf && result_path; 487 | } 488 | 489 | bool KeyframeMapper::savePcdMapSrvCallback( 490 | Save::Request& request, 491 | Save::Response& response) 492 | { 493 | ROS_INFO("Saving map as pcd..."); 494 | const std::string& path = request.filename; 495 | bool result = savePcdMap(path); 496 | 497 | if (result) ROS_INFO("Pcd map saved to %s", path.c_str()); 498 | else ROS_ERROR("Pcd map saving failed"); 499 | 500 | return result; 501 | } 502 | 503 | bool KeyframeMapper::saveOctomapSrvCallback( 504 | Save::Request& request, 505 | Save::Response& response) 506 | { 507 | ROS_INFO("Saving map as Octomap..."); 508 | const std::string& path = request.filename; 509 | bool result = saveOctomap(path); 510 | 511 | if (result) ROS_INFO("Octomap saved to %s", path.c_str()); 512 | else ROS_ERROR("Octomap saving failed"); 513 | 514 | return result; 515 | } 516 | 517 | bool KeyframeMapper::addManualKeyframeSrvCallback( 518 | AddManualKeyframe::Request& request, 519 | AddManualKeyframe::Response& response) 520 | { 521 | manual_add_ = true; 522 | 523 | return true; 524 | } 525 | 526 | bool KeyframeMapper::generateGraphSrvCallback( 527 | GenerateGraph::Request& request, 528 | GenerateGraph::Response& response) 529 | { 530 | associations_.clear(); 531 | graph_detector_.generateKeyframeAssociations(keyframes_, associations_); 532 | 533 | ROS_INFO("%d associations detected", (int)associations_.size()); 534 | 535 | publishKeyframePoses(); 536 | publishKeyframeAssociations(); 537 | 538 | return true; 539 | } 540 | 541 | bool KeyframeMapper::solveGraphSrvCallback( 542 | SolveGraph::Request& request, 543 | SolveGraph::Response& response) 544 | { 545 | ros::WallTime start = ros::WallTime::now(); 546 | 547 | // Graph solving: keyframe positions only, path is interpolated 548 | graph_solver_.solve(keyframes_, associations_); 549 | updatePathFromKeyframePoses(); 550 | 551 | // Graph solving: keyframe positions and VO path 552 | /* 553 | AffineTransformVector path; 554 | pathROSToEigenAffine(path_msg_, path); 555 | graph_solver_.solve(keyframes_, associations_, path); 556 | pathEigenAffineToROS(path, path_msg_); 557 | */ 558 | 559 | double dur = getMsDuration(start); 560 | 561 | ROS_INFO("Solving took %.1f ms", dur); 562 | 563 | publishPath(); 564 | publishKeyframePoses(); 565 | publishKeyframeAssociations(); 566 | 567 | return true; 568 | } 569 | 570 | 571 | /** In the event that the keyframe poses change (from pose-graph solving) 572 | * this function will propagete teh changes in the path message 573 | */ 574 | void KeyframeMapper::updatePathFromKeyframePoses() 575 | { 576 | int kf_size = keyframes_.size(); 577 | int f_size = path_msg_.poses.size(); 578 | 579 | // temporary store the new path 580 | AffineTransformVector path_new; 581 | path_new.resize(f_size); 582 | 583 | if (kf_size < 2) return; 584 | 585 | for (int kf_idx = 0; kf_idx < kf_size - 1; ++kf_idx) 586 | { 587 | // the indices of the current and next keyframes (a and b) 588 | const rgbdtools::RGBDKeyframe& keyframe_a = keyframes_[kf_idx]; 589 | const rgbdtools::RGBDKeyframe& keyframe_b = keyframes_[kf_idx + 1]; 590 | 591 | // the corresponding frame indices 592 | int f_idx_a = keyframe_a.index; 593 | int f_idx_b = keyframe_b.index; 594 | 595 | // the new poses of keyframes a and b (after graph solving) 596 | tf::Transform kf_pose_a = tfFromEigenAffine(keyframe_a.pose); 597 | tf::Transform kf_pose_b = tfFromEigenAffine(keyframe_b.pose); 598 | 599 | // the previous pose of keyframe a and b (before graph solving) 600 | tf::Transform kf_pose_a_prev, kf_pose_b_prev; 601 | tf::poseMsgToTF(path_msg_.poses[f_idx_a].pose, kf_pose_a_prev); 602 | tf::poseMsgToTF(path_msg_.poses[f_idx_b].pose, kf_pose_b_prev); 603 | 604 | // the motion, in the camera frame (after and before graph solving) 605 | tf::Transform kf_motion = kf_pose_a.inverse() * kf_pose_b; 606 | tf::Transform kf_motion_prev = kf_pose_a_prev.inverse() * kf_pose_b_prev; 607 | 608 | // the correction from the graph solving 609 | tf::Transform correction = kf_motion_prev.inverse() * kf_motion; 610 | 611 | // update the poses in-between keyframes 612 | for (int f_idx = f_idx_a; f_idx < f_idx_b; ++f_idx) 613 | { 614 | // calculate interpolation scale 615 | double interp_scale = (double)(f_idx - f_idx_a) / (double)(f_idx_b - f_idx_a); 616 | 617 | // create interpolated correction translation and rotation 618 | tf::Vector3 v_interp = correction.getOrigin() * interp_scale; 619 | tf::Quaternion q_interp = tf::Quaternion::getIdentity(); 620 | q_interp.slerp(correction.getRotation(), interp_scale); 621 | 622 | // create interpolated correction 623 | tf::Transform interpolated_correction; 624 | interpolated_correction.setOrigin(v_interp); 625 | interpolated_correction.setRotation(q_interp); 626 | 627 | // the previous frame pose 628 | tf::Transform frame_pose_prev; 629 | tf::poseMsgToTF(path_msg_.poses[f_idx].pose, frame_pose_prev); 630 | 631 | // the pevious frame motion 632 | tf::Transform frame_motion_prev = kf_pose_a_prev.inverse() * frame_pose_prev; 633 | 634 | // the interpolated motion 635 | tf::Transform interpolated_motion = frame_motion_prev * interpolated_correction; 636 | 637 | // calculate the interpolated pose 638 | path_new[f_idx] = keyframe_a.pose * eigenAffineFromTf(interpolated_motion); 639 | } 640 | } 641 | 642 | // update the last pose 643 | const rgbdtools::RGBDKeyframe& last_kf = keyframes_[kf_size - 1]; 644 | 645 | tf::Transform last_kf_pose_prev; 646 | tf::poseMsgToTF(path_msg_.poses[last_kf.index].pose, last_kf_pose_prev); 647 | 648 | // update the poses in-between last keyframe and end of vo path 649 | for (int f_idx = last_kf.index; f_idx < f_size; ++f_idx) 650 | { 651 | // the previous frame pose 652 | tf::Transform frame_pose_prev; 653 | tf::poseMsgToTF(path_msg_.poses[f_idx].pose, frame_pose_prev); 654 | 655 | // the pevious frame motion 656 | tf::Transform frame_motion_prev = last_kf_pose_prev.inverse() * frame_pose_prev; 657 | 658 | // calculate the new pose 659 | path_new[f_idx] = last_kf.pose * eigenAffineFromTf(frame_motion_prev); 660 | } 661 | 662 | // copy over the interpolated path 663 | pathEigenAffineToROS(path_new, path_msg_); 664 | } 665 | 666 | 667 | bool KeyframeMapper::savePcdMap(const std::string& path) 668 | { 669 | PointCloudT pcd_map; 670 | buildPcdMap(pcd_map); 671 | 672 | // write out 673 | pcl::PCDWriter writer; 674 | int result_pcd = writer.writeBinary(path, pcd_map); 675 | 676 | if (result_pcd < 0) return false; 677 | else return true; 678 | } 679 | 680 | void KeyframeMapper::buildPcdMap(PointCloudT& map_cloud) 681 | { 682 | PointCloudT::Ptr aggregate_cloud(new PointCloudT()); 683 | aggregate_cloud->header.frame_id = fixed_frame_; 684 | 685 | // aggregate all frames into single cloud 686 | for (unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx) 687 | { 688 | const rgbdtools::RGBDKeyframe& keyframe = keyframes_[kf_idx]; 689 | 690 | PointCloudT cloud; 691 | keyframe.constructDensePointCloud(cloud, max_range_, max_stdev_); 692 | 693 | PointCloudT cloud_tf; 694 | pcl::transformPointCloud(cloud, cloud_tf, keyframe.pose); 695 | cloud_tf.header.frame_id = fixed_frame_; 696 | 697 | *aggregate_cloud += cloud_tf; 698 | } 699 | 700 | // filter cloud using voxel grid, and for max z 701 | pcl::VoxelGrid vgf; 702 | vgf.setInputCloud(aggregate_cloud); 703 | vgf.setLeafSize(pcd_map_res_, pcd_map_res_, pcd_map_res_); 704 | vgf.setFilterFieldName("z"); 705 | vgf.setFilterLimits (-std::numeric_limits::infinity(), max_map_z_); 706 | 707 | vgf.filter(map_cloud); 708 | } 709 | 710 | bool KeyframeMapper::saveOctomap(const std::string& path) 711 | { 712 | bool result; 713 | 714 | if (octomap_with_color_) 715 | { 716 | octomap::ColorOcTree tree(octomap_res_); 717 | buildColorOctomap(tree); 718 | result = tree.write(path); 719 | } 720 | else 721 | { 722 | octomap::OcTree tree(octomap_res_); 723 | buildOctomap(tree); 724 | result = tree.write(path); 725 | } 726 | 727 | return result; 728 | } 729 | 730 | void KeyframeMapper::buildOctomap(octomap::OcTree& tree) 731 | { 732 | ROS_INFO("Building Octomap..."); 733 | 734 | octomap::point3d sensor_origin(0.0, 0.0, 0.0); 735 | 736 | for (unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx) 737 | { 738 | ROS_INFO("Processing keyframe %u", kf_idx); 739 | const rgbdtools::RGBDKeyframe& keyframe = keyframes_[kf_idx]; 740 | 741 | PointCloudT cloud; 742 | keyframe.constructDensePointCloud(cloud, max_range_, max_stdev_); 743 | 744 | octomap::pose6d frame_origin = poseTfToOctomap(tfFromEigenAffine(keyframe.pose)); 745 | 746 | // build octomap cloud from pcl cloud 747 | octomap::Pointcloud octomap_cloud; 748 | for (unsigned int pt_idx = 0; pt_idx < cloud.points.size(); ++pt_idx) 749 | { 750 | const PointT& p = cloud.points[pt_idx]; 751 | if (!std::isnan(p.z)) 752 | octomap_cloud.push_back(p.x, p.y, p.z); 753 | } 754 | 755 | tree.insertScan(octomap_cloud, sensor_origin, frame_origin); 756 | } 757 | } 758 | 759 | void KeyframeMapper::buildColorOctomap(octomap::ColorOcTree& tree) 760 | { 761 | ROS_INFO("Building Octomap with color..."); 762 | 763 | octomap::point3d sensor_origin(0.0, 0.0, 0.0); 764 | 765 | for (unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx) 766 | { 767 | ROS_INFO("Processing keyframe %u", kf_idx); 768 | const rgbdtools::RGBDKeyframe& keyframe = keyframes_[kf_idx]; 769 | 770 | // construct the cloud 771 | PointCloudT::Ptr cloud_unf(new PointCloudT()); 772 | keyframe.constructDensePointCloud(*cloud_unf, max_range_, max_stdev_); 773 | 774 | // perform filtering for max z 775 | pcl::transformPointCloud(*cloud_unf, *cloud_unf, keyframe.pose); 776 | PointCloudT cloud; 777 | pcl::PassThrough pass; 778 | pass.setInputCloud (cloud_unf); 779 | pass.setFilterFieldName ("z"); 780 | pass.setFilterLimits (-std::numeric_limits::infinity(), max_map_z_); 781 | pass.filter(cloud); 782 | pcl::transformPointCloud(cloud, cloud, keyframe.pose.inverse()); 783 | 784 | octomap::pose6d frame_origin = poseTfToOctomap(tfFromEigenAffine(keyframe.pose)); 785 | 786 | // build octomap cloud from pcl cloud 787 | octomap::Pointcloud octomap_cloud; 788 | for (unsigned int pt_idx = 0; pt_idx < cloud.points.size(); ++pt_idx) 789 | { 790 | const PointT& p = cloud.points[pt_idx]; 791 | if (!std::isnan(p.z)) 792 | octomap_cloud.push_back(p.x, p.y, p.z); 793 | } 794 | 795 | // insert scan (only xyz considered, no colors) 796 | tree.insertScan(octomap_cloud, sensor_origin, frame_origin); 797 | 798 | // insert colors 799 | PointCloudT cloud_tf; 800 | pcl::transformPointCloud(cloud, cloud_tf, keyframe.pose); 801 | for (unsigned int pt_idx = 0; pt_idx < cloud_tf.points.size(); ++pt_idx) 802 | { 803 | const PointT& p = cloud_tf.points[pt_idx]; 804 | if (!std::isnan(p.z)) 805 | { 806 | octomap::point3d endpoint(p.x, p.y, p.z); 807 | octomap::ColorOcTreeNode* n = tree.search(endpoint); 808 | if (n) n->setColor(p.r, p.g, p.b); 809 | } 810 | } 811 | 812 | tree.updateInnerOccupancy(); 813 | } 814 | } 815 | 816 | void KeyframeMapper::publishPath() 817 | { 818 | path_msg_.header.frame_id = fixed_frame_; 819 | path_pub_.publish(path_msg_); 820 | } 821 | 822 | bool KeyframeMapper::savePath(const std::string& filepath) 823 | { 824 | // open file 825 | std::string filename = filepath + "/path.txt"; 826 | std::ofstream file(filename.c_str()); 827 | if (!file.is_open()) return false; 828 | 829 | file << "# index seq stamp.sec stamp.nsec x y z qx qy qz qw" << std::endl; 830 | 831 | for (unsigned int idx = 0; idx < path_msg_.poses.size(); ++idx) 832 | { 833 | const geometry_msgs::PoseStamped& pose = path_msg_.poses[idx]; 834 | 835 | file << idx << " " 836 | << pose.header.seq << " " 837 | << pose.header.stamp.sec << " " 838 | << pose.header.stamp.nsec << " " 839 | << pose.pose.position.x << " " 840 | << pose.pose.position.y << " " 841 | << pose.pose.position.z << " " 842 | << pose.pose.orientation.x << " " 843 | << pose.pose.orientation.y << " " 844 | << pose.pose.orientation.z << " " 845 | << pose.pose.orientation.w << std::endl; 846 | } 847 | 848 | file.close(); 849 | 850 | return true; 851 | } 852 | 853 | bool KeyframeMapper::savePathTUMFormat(const std::string& filepath) 854 | { 855 | // open file 856 | std::string filename = filepath + "/path.tum.txt"; 857 | std::ofstream file(filename.c_str()); 858 | if (!file.is_open()) return false; 859 | 860 | file << "# stamp x y z qx qy qz qw" << std::endl; 861 | 862 | for (unsigned int idx = 0; idx < path_msg_.poses.size(); ++idx) 863 | { 864 | const geometry_msgs::PoseStamped& pose = path_msg_.poses[idx]; 865 | 866 | file << pose.header.stamp.sec << "." 867 | << pose.header.stamp.nsec << " " 868 | << pose.pose.position.x << " " 869 | << pose.pose.position.y << " " 870 | << pose.pose.position.z << " " 871 | << pose.pose.orientation.x << " " 872 | << pose.pose.orientation.y << " " 873 | << pose.pose.orientation.z << " " 874 | << pose.pose.orientation.w << std::endl; 875 | } 876 | 877 | file.close(); 878 | 879 | return true; 880 | } 881 | 882 | bool KeyframeMapper::loadPath(const std::string& filepath) 883 | { 884 | path_msg_.poses.clear(); 885 | 886 | // open file 887 | std::string filename = filepath + "/path.txt"; 888 | std::ifstream file(filename.c_str()); 889 | if (!file.is_open()) return false; 890 | 891 | std::string line; 892 | 893 | // get header 894 | getline(file, line); 895 | 896 | // read each line 897 | while(std::getline(file, line)) 898 | { 899 | std::istringstream is(line); 900 | 901 | // fill out pose information 902 | geometry_msgs::PoseStamped pose; 903 | pose.header.frame_id = fixed_frame_; 904 | int idx; 905 | 906 | is >> idx 907 | >> pose.header.seq 908 | >> pose.header.stamp.sec 909 | >> pose.header.stamp.nsec 910 | >> pose.pose.position.x 911 | >> pose.pose.position.y 912 | >> pose.pose.position.z 913 | >> pose.pose.orientation.x 914 | >> pose.pose.orientation.y 915 | >> pose.pose.orientation.z 916 | >> pose.pose.orientation.w; 917 | 918 | // add to poses vector 919 | path_msg_.poses.push_back(pose); 920 | } 921 | 922 | file.close(); 923 | return true; 924 | } 925 | 926 | } // namespace ccny_rgbd 927 | -------------------------------------------------------------------------------- /ccny_rgbd/src/apps/rgbd_image_proc.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_image_proc.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "ccny_rgbd/apps/rgbd_image_proc.h" 25 | 26 | namespace ccny_rgbd { 27 | 28 | RGBDImageProc::RGBDImageProc( 29 | const ros::NodeHandle& nh, 30 | const ros::NodeHandle& nh_private): 31 | nh_(nh), nh_private_(nh_private), 32 | rgb_image_transport_(nh_), 33 | depth_image_transport_(nh_), 34 | config_server_(nh_private_), 35 | size_in_(0,0) 36 | { 37 | // parameters 38 | if (!nh_private_.getParam ("queue_size", queue_size_)) 39 | queue_size_ = 5; 40 | if (!nh_private_.getParam("scale", scale_)) 41 | scale_ = 1.0; 42 | if (!nh_private_.getParam("unwarp", unwarp_)) 43 | unwarp_ = true; 44 | if (!nh_private_.getParam("verbose", verbose_)) 45 | verbose_ = false; 46 | if (!nh_private_.getParam("publish_cloud", publish_cloud_)) 47 | publish_cloud_ = true; 48 | if (!nh_private_.getParam("calib_path", calib_path_)) 49 | { 50 | std::string home_path = getenv("HOME"); 51 | calib_path_ = home_path + "./ros/rgbd_calibration"; 52 | } 53 | 54 | calib_extr_filename_ = calib_path_ + "/extr.yml"; 55 | calib_warp_filename_ = calib_path_ + "/warp.yml"; 56 | 57 | // load calibration (extrinsics, depth unwarp params) from files 58 | loadCalibration(); 59 | if (unwarp_) 60 | { 61 | bool load_result = loadUnwarpCalibration(); 62 | if (!load_result) 63 | { 64 | ROS_WARN("Disbaling unwarping due to missing calibration file"); 65 | unwarp_ = true; 66 | } 67 | } 68 | 69 | // publishers 70 | rgb_publisher_ = rgb_image_transport_.advertise( 71 | "rgbd/rgb", queue_size_); 72 | depth_publisher_ = depth_image_transport_.advertise( 73 | "rgbd/depth", queue_size_); 74 | info_publisher_ = nh_.advertise( 75 | "rgbd/info", queue_size_); 76 | 77 | if(publish_cloud_) 78 | cloud_publisher_ = nh_.advertise( 79 | "rgbd/cloud", queue_size_); 80 | 81 | // dynamic reconfigure 82 | ProcConfigServer::CallbackType f = boost::bind(&RGBDImageProc::reconfigCallback, this, _1, _2); 83 | config_server_.setCallback(f); 84 | 85 | // subscribers 86 | sub_rgb_.subscribe (rgb_image_transport_, 87 | "/camera/rgb/image_color", queue_size_); 88 | sub_depth_.subscribe(depth_image_transport_, 89 | "/camera/depth/image_raw", queue_size_); //16UC1 90 | 91 | sub_rgb_info_.subscribe (nh_, 92 | "/camera/rgb/camera_info", queue_size_); 93 | sub_depth_info_.subscribe(nh_, 94 | "/camera/depth/camera_info", queue_size_); 95 | 96 | sync_.reset(new RGBDSynchronizer4( 97 | RGBDSyncPolicy4(queue_size_), sub_rgb_, sub_depth_, 98 | sub_rgb_info_, sub_depth_info_)); 99 | 100 | sync_->registerCallback(boost::bind(&RGBDImageProc::RGBDCallback, this, _1, _2, _3, _4)); 101 | } 102 | 103 | RGBDImageProc::~RGBDImageProc() 104 | { 105 | ROS_INFO("Destroying RGBDImageProc"); 106 | } 107 | 108 | bool RGBDImageProc::loadCalibration() 109 | { 110 | // check that file exist 111 | if (!boost::filesystem::exists(calib_extr_filename_)) 112 | { 113 | ROS_ERROR("Could not open %s", calib_extr_filename_.c_str()); 114 | /// @todo handle this with default ir2rgb 115 | return false; 116 | } 117 | 118 | // load intrinsics and distortion coefficients 119 | ROS_INFO("Reading camera calibration files..."); 120 | cv::FileStorage fs_extr(calib_extr_filename_, cv::FileStorage::READ); 121 | 122 | fs_extr["ir2rgb"] >> ir2rgb_; 123 | 124 | return true; 125 | } 126 | 127 | bool RGBDImageProc::loadUnwarpCalibration() 128 | { 129 | if (!boost::filesystem::exists(calib_warp_filename_)) 130 | { 131 | ROS_ERROR("Could not open %s", calib_warp_filename_.c_str()); 132 | return false; 133 | } 134 | 135 | // load warp coefficients 136 | cv::FileStorage fs_warp(calib_warp_filename_, cv::FileStorage::READ); 137 | 138 | fs_warp["c0"] >> coeff_0_; 139 | fs_warp["c1"] >> coeff_1_; 140 | fs_warp["c2"] >> coeff_2_; 141 | fs_warp["fit_mode"] >> fit_mode_; 142 | 143 | return true; 144 | } 145 | 146 | void RGBDImageProc::initMaps( 147 | const CameraInfoMsg::ConstPtr& rgb_info_msg, 148 | const CameraInfoMsg::ConstPtr& depth_info_msg) 149 | { 150 | boost::mutex::scoped_lock(mutex_); 151 | 152 | ROS_INFO("Initializing rectification maps"); 153 | 154 | // **** get OpenCV matrices from CameraInfo messages 155 | cv::Mat intr_rgb, intr_depth; 156 | cv::Mat dist_rgb, dist_depth; 157 | 158 | convertCameraInfoToMats(rgb_info_msg, intr_rgb, dist_rgb); 159 | convertCameraInfoToMats(depth_info_msg, intr_depth, dist_depth); 160 | 161 | // **** sizes 162 | double alpha = 0.0; 163 | 164 | if (size_in_.width != (int)rgb_info_msg->width || 165 | size_in_.height != (int)rgb_info_msg->height) 166 | { 167 | ROS_WARN("Image size does not match CameraInfo size. Rescaling."); 168 | double w_factor = (double)size_in_.width / (double)rgb_info_msg->width; 169 | double h_factor = (double)size_in_.height / (double)rgb_info_msg->height; 170 | 171 | intr_rgb.at(0,0) *= w_factor; 172 | intr_rgb.at(1,1) *= h_factor; 173 | intr_rgb.at(0,2) *= w_factor; 174 | intr_rgb.at(1,2) *= h_factor; 175 | 176 | intr_depth.at(0,0) *= w_factor; 177 | intr_depth.at(1,1) *= h_factor; 178 | intr_depth.at(0,2) *= w_factor; 179 | intr_depth.at(1,2) *= h_factor; 180 | } 181 | 182 | cv::Size size_out; 183 | size_out.height = size_in_.height * scale_; 184 | size_out.width = size_in_.width * scale_; 185 | 186 | // **** get optimal camera matrices 187 | intr_rect_rgb_ = cv::getOptimalNewCameraMatrix( 188 | intr_rgb, dist_rgb, size_in_, alpha, size_out); 189 | 190 | intr_rect_depth_ = cv::getOptimalNewCameraMatrix( 191 | intr_depth, dist_depth, size_in_, alpha, size_out); 192 | 193 | // **** create undistortion maps 194 | cv::initUndistortRectifyMap( 195 | intr_rgb, dist_rgb, cv::Mat(), intr_rect_rgb_, 196 | size_out, CV_16SC2, map_rgb_1_, map_rgb_2_); 197 | 198 | cv::initUndistortRectifyMap( 199 | intr_depth, dist_depth, cv::Mat(), intr_rect_depth_, 200 | size_out, CV_16SC2, map_depth_1_, map_depth_2_); 201 | 202 | // **** rectify the coefficient images 203 | if(unwarp_) 204 | { 205 | cv::remap(coeff_0_, coeff_0_rect_, map_depth_1_, map_depth_2_, cv::INTER_NEAREST); 206 | cv::remap(coeff_1_, coeff_1_rect_, map_depth_1_, map_depth_2_, cv::INTER_NEAREST); 207 | cv::remap(coeff_2_, coeff_2_rect_, map_depth_1_, map_depth_2_, cv::INTER_NEAREST); 208 | } 209 | 210 | // **** save new intrinsics as camera models 211 | rgb_rect_info_msg_.header = rgb_info_msg->header; 212 | rgb_rect_info_msg_.width = size_out.width; 213 | rgb_rect_info_msg_.height = size_out.height; 214 | 215 | depth_rect_info_msg_.header = depth_info_msg->header; 216 | depth_rect_info_msg_.width = size_out.width; 217 | depth_rect_info_msg_.height = size_out.height; 218 | 219 | convertMatToCameraInfo(intr_rect_rgb_, rgb_rect_info_msg_); 220 | convertMatToCameraInfo(intr_rect_depth_, depth_rect_info_msg_); 221 | } 222 | 223 | void RGBDImageProc::RGBDCallback( 224 | const ImageMsg::ConstPtr& rgb_msg, 225 | const ImageMsg::ConstPtr& depth_msg, 226 | const CameraInfoMsg::ConstPtr& rgb_info_msg, 227 | const CameraInfoMsg::ConstPtr& depth_info_msg) 228 | { 229 | boost::mutex::scoped_lock(mutex_); 230 | 231 | // for profiling 232 | double dur_unwarp, dur_rectify, dur_reproject, dur_cloud, dur_allocate; 233 | 234 | // **** images need to be the same size 235 | if (rgb_msg->height != depth_msg->height || 236 | rgb_msg->width != depth_msg->width) 237 | { 238 | ROS_WARN("RGB and depth images have different sizes, skipping"); 239 | return; 240 | } 241 | 242 | // **** initialize if needed 243 | if (size_in_.height != (int)rgb_msg->height || 244 | size_in_.width != (int)rgb_msg->width) 245 | { 246 | ROS_INFO("Initializing"); 247 | 248 | size_in_.height = (int)rgb_msg->height; 249 | size_in_.width = (int)rgb_msg->width; 250 | 251 | initMaps(rgb_info_msg, depth_info_msg); 252 | } 253 | 254 | // **** convert ros images to opencv Mat 255 | cv_bridge::CvImageConstPtr rgb_ptr = cv_bridge::toCvShare(rgb_msg); 256 | cv_bridge::CvImageConstPtr depth_ptr = cv_bridge::toCvShare(depth_msg); 257 | 258 | const cv::Mat& rgb_img = rgb_ptr->image; 259 | const cv::Mat& depth_img = depth_ptr->image; 260 | 261 | //cv::imshow("RGB", rgb_img); 262 | //cv::imshow("Depth", depth_img); 263 | //cv::waitKey(1); 264 | 265 | // **** rectify 266 | ros::WallTime start_rectify = ros::WallTime::now(); 267 | cv::Mat rgb_img_rect, depth_img_rect; 268 | cv::remap(rgb_img, rgb_img_rect, map_rgb_1_, map_rgb_2_, cv::INTER_LINEAR); 269 | cv::remap(depth_img, depth_img_rect, map_depth_1_, map_depth_2_, cv::INTER_NEAREST); 270 | dur_rectify = getMsDuration(start_rectify); 271 | 272 | //cv::imshow("RGB Rect", rgb_img_rect); 273 | //cv::imshow("Depth Rect", depth_img_rect); 274 | //cv::waitKey(1); 275 | 276 | // **** unwarp 277 | if (unwarp_) 278 | { 279 | ros::WallTime start_unwarp = ros::WallTime::now(); 280 | rgbdtools::unwarpDepthImage( 281 | depth_img_rect, coeff_0_rect_, coeff_1_rect_, coeff_2_rect_, fit_mode_); 282 | dur_unwarp = getMsDuration(start_unwarp); 283 | } 284 | else dur_unwarp = 0.0; 285 | 286 | // **** reproject 287 | ros::WallTime start_reproject = ros::WallTime::now(); 288 | cv::Mat depth_img_rect_reg; 289 | rgbdtools::buildRegisteredDepthImage( 290 | intr_rect_depth_, intr_rect_rgb_, ir2rgb_, depth_img_rect, depth_img_rect_reg); 291 | dur_reproject = getMsDuration(start_reproject); 292 | 293 | // **** point cloud 294 | if (publish_cloud_) 295 | { 296 | ros::WallTime start_cloud = ros::WallTime::now(); 297 | PointCloudT::Ptr cloud_ptr; 298 | cloud_ptr.reset(new PointCloudT()); 299 | rgbdtools::buildPointCloud( 300 | depth_img_rect_reg, rgb_img_rect, intr_rect_rgb_, *cloud_ptr); 301 | // The point cloud timestamp, int usec. 302 | cloud_ptr->header.stamp = rgb_info_msg->header.stamp.toNSec() * 1e-3; 303 | cloud_ptr->header.frame_id = rgb_info_msg->header.frame_id; 304 | cloud_publisher_.publish(cloud_ptr); 305 | dur_cloud = getMsDuration(start_cloud); 306 | } 307 | else dur_cloud = 0.0; 308 | 309 | // **** allocate registered rgb image 310 | ros::WallTime start_allocate = ros::WallTime::now(); 311 | 312 | cv_bridge::CvImage cv_img_rgb(rgb_msg->header, rgb_msg->encoding, rgb_img_rect); 313 | ImageMsg::Ptr rgb_out_msg = cv_img_rgb.toImageMsg(); 314 | 315 | // **** allocate registered depth image 316 | cv_bridge::CvImage cv_img_depth(depth_msg->header, depth_msg->encoding, depth_img_rect_reg); 317 | ImageMsg::Ptr depth_out_msg = cv_img_depth.toImageMsg(); 318 | 319 | // **** update camera info (single, since both images are in rgb frame) 320 | rgb_rect_info_msg_.header = rgb_info_msg->header; 321 | 322 | dur_allocate = getMsDuration(start_allocate); 323 | 324 | // **** print diagnostics 325 | 326 | double dur_total = dur_rectify + dur_reproject + dur_unwarp + dur_cloud + dur_allocate; 327 | if(verbose_) 328 | { 329 | ROS_INFO("Rect %.1f Reproj %.1f Unwarp %.1f Cloud %.1f Alloc %.1f Total %.1f ms", 330 | dur_rectify, dur_reproject, dur_unwarp, dur_cloud, dur_allocate, 331 | dur_total); 332 | } 333 | // **** publish 334 | rgb_publisher_.publish(rgb_out_msg); 335 | depth_publisher_.publish(depth_out_msg); 336 | info_publisher_.publish(rgb_rect_info_msg_); 337 | } 338 | 339 | void RGBDImageProc::reconfigCallback(ProcConfig& config, uint32_t level) 340 | { 341 | boost::mutex::scoped_lock(mutex_); 342 | bool old_publish_cloud = publish_cloud_; 343 | publish_cloud_ = config.publish_cloud; 344 | if(!old_publish_cloud && publish_cloud_) 345 | { 346 | cloud_publisher_ = nh_.advertise( 347 | "rgbd/cloud", queue_size_); 348 | } 349 | else 350 | { 351 | if(old_publish_cloud && !publish_cloud_) 352 | cloud_publisher_.shutdown(); 353 | } 354 | 355 | 356 | scale_ = config.scale; 357 | size_in_ = cv::Size(0,0); // force a reinitialization on the next image callback 358 | ROS_INFO("Resampling scale set to %.2f", scale_); 359 | } 360 | 361 | } //namespace ccny_rgbd 362 | -------------------------------------------------------------------------------- /ccny_rgbd/src/apps/visual_odometry.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file visual_odometry.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "ccny_rgbd/apps/visual_odometry.h" 25 | 26 | namespace ccny_rgbd { 27 | 28 | VisualOdometry::VisualOdometry( 29 | const ros::NodeHandle& nh, 30 | const ros::NodeHandle& nh_private): 31 | nh_(nh), 32 | nh_private_(nh_private), 33 | initialized_(false), 34 | frame_count_(0) 35 | { 36 | ROS_INFO("Starting RGBD Visual Odometry"); 37 | 38 | // **** initialize ROS parameters 39 | 40 | initParams(); 41 | 42 | // **** inititialize state variables 43 | 44 | f2b_.setIdentity(); 45 | 46 | // **** publishers 47 | 48 | odom_publisher_ = nh_.advertise( 49 | "vo", queue_size_); 50 | pose_stamped_publisher_ = nh_.advertise( 51 | "pose", queue_size_); 52 | path_pub_ = nh_.advertise( 53 | "path", queue_size_); 54 | 55 | feature_cloud_publisher_ = nh_.advertise( 56 | "feature/cloud", 1); 57 | feature_cov_publisher_ = nh_.advertise( 58 | "feature/covariances", 1); 59 | 60 | model_cloud_publisher_ = nh_.advertise( 61 | "model/cloud", 1); 62 | model_cov_publisher_ = nh_.advertise( 63 | "model/covariances", 1); 64 | 65 | // **** subscribers 66 | 67 | ImageTransport rgb_it(nh_); 68 | ImageTransport depth_it(nh_); 69 | 70 | sub_rgb_.subscribe(rgb_it, "/rgbd/rgb", queue_size_); 71 | sub_depth_.subscribe(depth_it, "/rgbd/depth", queue_size_); 72 | sub_info_.subscribe(nh_, "/rgbd/info", queue_size_); 73 | 74 | // Synchronize inputs. 75 | sync_.reset(new RGBDSynchronizer3( 76 | RGBDSyncPolicy3(queue_size_), sub_rgb_, sub_depth_, sub_info_)); 77 | 78 | sync_->registerCallback(boost::bind(&VisualOdometry::RGBDCallback, this, _1, _2, _3)); 79 | } 80 | 81 | VisualOdometry::~VisualOdometry() 82 | { 83 | fclose(diagnostics_file_); 84 | ROS_INFO("Destroying RGBD Visual Odometry"); 85 | } 86 | 87 | void VisualOdometry::initParams() 88 | { 89 | if (!nh_private_.getParam ("publish_tf", publish_tf_)) 90 | publish_tf_ = true; 91 | if (!nh_private_.getParam ("publish_path", publish_path_)) 92 | publish_path_ = true; 93 | if (!nh_private_.getParam ("publish_odom", publish_odom_)) 94 | publish_odom_ = true; 95 | if (!nh_private_.getParam ("publish_pose", publish_pose_)) 96 | publish_pose_ = true; 97 | if (!nh_private_.getParam ("fixed_frame", fixed_frame_)) 98 | fixed_frame_ = "/odom"; 99 | if (!nh_private_.getParam ("base_frame", base_frame_)) 100 | base_frame_ = "/camera_link"; 101 | if (!nh_private_.getParam ("queue_size", queue_size_)) 102 | queue_size_ = 5; 103 | 104 | // detector params 105 | 106 | if (!nh_private_.getParam ("feature/publish_feature_cloud", publish_feature_cloud_)) 107 | publish_feature_cloud_ = false; 108 | if (!nh_private_.getParam ("feature/publish_feature_covariances", publish_feature_cov_)) 109 | publish_feature_cov_ = false; 110 | if (!nh_private_.getParam ("feature/detector_type", detector_type_)) 111 | detector_type_ = "GFT"; 112 | 113 | resetDetector(); 114 | 115 | int smooth; 116 | double max_range, max_stdev; 117 | 118 | if (!nh_private_.getParam ("feature/smooth", smooth)) 119 | smooth = 0; 120 | if (!nh_private_.getParam ("feature/max_range", max_range)) 121 | max_range = 5.5; 122 | if (!nh_private_.getParam ("feature/max_stdev", max_stdev)) 123 | max_stdev = 0.03; 124 | 125 | feature_detector_->setSmooth(smooth); 126 | feature_detector_->setMaxRange(max_range); 127 | feature_detector_->setMaxStDev(max_stdev); 128 | 129 | // registration params 130 | 131 | configureMotionEstimation(); 132 | 133 | // diagnostic params 134 | 135 | if (!nh_private_.getParam("verbose", verbose_)) 136 | verbose_ = true; 137 | if (!nh_private_.getParam("save_diagnostics", save_diagnostics_)) 138 | save_diagnostics_ = false; 139 | if (!nh_private_.getParam("diagnostics_file_name", diagnostics_file_name_)) 140 | diagnostics_file_name_ = "diagnostics.csv"; 141 | 142 | if(save_diagnostics_) 143 | { 144 | diagnostics_file_ = fopen(diagnostics_file_name_.c_str(), "w"); 145 | 146 | if (diagnostics_file_ == NULL) 147 | { 148 | ROS_ERROR("Can't create diagnostic file %s\n", diagnostics_file_name_.c_str()); 149 | return; 150 | } 151 | 152 | // print header 153 | fprintf(diagnostics_file_, "%s, %s, %s, %s, %s, %s, %s, %s\n", 154 | "Frame id", 155 | "Frame dur.", 156 | "All features", "Valid features", 157 | "Feat extr. dur.", 158 | "Model points", "Registration dur.", 159 | "Total dur."); 160 | } 161 | } 162 | 163 | void VisualOdometry::configureMotionEstimation() 164 | { 165 | int motion_constraint; 166 | 167 | if (!nh_private_.getParam ("reg/motion_constraint", motion_constraint)) 168 | motion_constraint = 0; 169 | 170 | motion_estimation_.setMotionConstraint(motion_constraint); 171 | 172 | double tf_epsilon_linear; 173 | double tf_epsilon_angular; 174 | int max_iterations; 175 | int min_correspondences; 176 | int max_model_size; 177 | double max_corresp_dist_eucl; 178 | double max_assoc_dist_mah; 179 | int n_nearest_neighbors; 180 | 181 | if (!nh_private_.getParam ("reg/ICPProbModel/tf_epsilon_linear", tf_epsilon_linear)) 182 | tf_epsilon_linear = 1e-4; // 1 mm 183 | if (!nh_private_.getParam ("reg/ICPProbModel/tf_epsilon_angular", tf_epsilon_angular)) 184 | tf_epsilon_angular = 1.7e-3; // 1 deg 185 | if (!nh_private_.getParam ("reg/ICPProbModel/max_iterations", max_iterations)) 186 | max_iterations = 10; 187 | if (!nh_private_.getParam ("reg/ICPProbModel/min_correspondences", min_correspondences)) 188 | min_correspondences = 15; 189 | if (!nh_private_.getParam ("reg/ICPProbModel/max_model_size", max_model_size)) 190 | max_model_size = 3000; 191 | if (!nh_private_.getParam ("reg/ICPProbModel/max_corresp_dist_eucl", max_corresp_dist_eucl)) 192 | max_corresp_dist_eucl = 0.15; 193 | if (!nh_private_.getParam ("reg/ICPProbModel/max_assoc_dist_mah", max_assoc_dist_mah)) 194 | max_assoc_dist_mah = 10.0; 195 | if (!nh_private_.getParam ("reg/ICPProbModel/n_nearest_neighbors", n_nearest_neighbors)) 196 | n_nearest_neighbors = 4; 197 | 198 | if (!nh_private_.getParam ("reg/ICPProbModel/publish_model_cloud", publish_model_cloud_)) 199 | publish_model_cloud_ = false; 200 | if (!nh_private_.getParam ("reg/ICPProbModel/publish_model_covariances", publish_model_cov_)) 201 | publish_model_cov_ = false; 202 | 203 | motion_estimation_.setTfEpsilonLinear(tf_epsilon_linear); 204 | motion_estimation_.setTfEpsilonAngular(tf_epsilon_angular); 205 | motion_estimation_.setMaxIterations(max_iterations); 206 | motion_estimation_.setMinCorrespondences(min_correspondences); 207 | motion_estimation_.setMaxModelSize(max_model_size); 208 | motion_estimation_.setMaxCorrespondenceDistEuclidean(max_corresp_dist_eucl); 209 | motion_estimation_.setMaxAssociationDistMahalanobis(max_assoc_dist_mah); 210 | motion_estimation_.setNNearestNeighbors(n_nearest_neighbors); 211 | } 212 | 213 | void VisualOdometry::resetDetector() 214 | { 215 | gft_config_server_.reset(); 216 | star_config_server_.reset(); 217 | orb_config_server_.reset(); 218 | 219 | if (detector_type_ == "ORB") 220 | { 221 | ROS_INFO("Creating ORB detector"); 222 | feature_detector_.reset(new rgbdtools::OrbDetector()); 223 | orb_config_server_.reset(new 224 | OrbDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/ORB"))); 225 | 226 | // dynamic reconfigure 227 | OrbDetectorConfigServer::CallbackType f = boost::bind( 228 | &VisualOdometry::orbReconfigCallback, this, _1, _2); 229 | orb_config_server_->setCallback(f); 230 | } 231 | else if (detector_type_ == "GFT") 232 | { 233 | ROS_INFO("Creating GFT detector"); 234 | feature_detector_.reset(new rgbdtools::GftDetector()); 235 | gft_config_server_.reset(new 236 | GftDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/GFT"))); 237 | 238 | // dynamic reconfigure 239 | GftDetectorConfigServer::CallbackType f = boost::bind( 240 | &VisualOdometry::gftReconfigCallback, this, _1, _2); 241 | gft_config_server_->setCallback(f); 242 | } 243 | else if (detector_type_ == "STAR") 244 | { 245 | ROS_INFO("Creating STAR detector"); 246 | feature_detector_.reset(new rgbdtools::StarDetector()); 247 | star_config_server_.reset(new 248 | StarDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/STAR"))); 249 | 250 | // dynamic reconfigure 251 | StarDetectorConfigServer::CallbackType f = boost::bind( 252 | &VisualOdometry::starReconfigCallback, this, _1, _2); 253 | star_config_server_->setCallback(f); 254 | } 255 | else 256 | { 257 | ROS_FATAL("%s is not a valid detector type! Using GFT", detector_type_.c_str()); 258 | feature_detector_.reset(new rgbdtools::GftDetector()); 259 | gft_config_server_.reset(new 260 | GftDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/GFT"))); 261 | 262 | // dynamic reconfigure 263 | GftDetectorConfigServer::CallbackType f = boost::bind( 264 | &VisualOdometry::gftReconfigCallback, this, _1, _2); 265 | gft_config_server_->setCallback(f); 266 | } 267 | } 268 | 269 | void VisualOdometry::RGBDCallback( 270 | const ImageMsg::ConstPtr& rgb_msg, 271 | const ImageMsg::ConstPtr& depth_msg, 272 | const CameraInfoMsg::ConstPtr& info_msg) 273 | { 274 | ros::WallTime start = ros::WallTime::now(); 275 | 276 | // **** initialize *************************************************** 277 | 278 | if (!initialized_) 279 | { 280 | initialized_ = getBaseToCameraTf(rgb_msg->header); 281 | init_time_ = rgb_msg->header.stamp; 282 | if (!initialized_) return; 283 | 284 | motion_estimation_.setBaseToCameraTf(eigenAffineFromTf(b2c_)); 285 | } 286 | 287 | // **** create frame ************************************************* 288 | 289 | ros::WallTime start_frame = ros::WallTime::now(); 290 | rgbdtools::RGBDFrame frame; 291 | createRGBDFrameFromROSMessages(rgb_msg, depth_msg, info_msg, frame); 292 | ros::WallTime end_frame = ros::WallTime::now(); 293 | 294 | // **** find features ************************************************ 295 | 296 | ros::WallTime start_features = ros::WallTime::now(); 297 | feature_detector_->findFeatures(frame); 298 | ros::WallTime end_features = ros::WallTime::now(); 299 | 300 | // **** registration ************************************************* 301 | 302 | ros::WallTime start_reg = ros::WallTime::now(); 303 | AffineTransform m = motion_estimation_.getMotionEstimation(frame); 304 | tf::Transform motion = tfFromEigenAffine(m); 305 | f2b_ = motion * f2b_; 306 | ros::WallTime end_reg = ros::WallTime::now(); 307 | 308 | // **** publish outputs ********************************************** 309 | 310 | if (publish_tf_) publishTf(rgb_msg->header); 311 | if (publish_odom_) publishOdom(rgb_msg->header); 312 | if (publish_path_) publishPath(rgb_msg->header); 313 | if (publish_pose_) publishPoseStamped(rgb_msg->header); 314 | 315 | if (publish_feature_cloud_) publishFeatureCloud(frame); 316 | if (publish_feature_cov_) publishFeatureCovariances(frame); 317 | 318 | if (publish_model_cloud_) publishModelCloud(); 319 | if (publish_model_cov_) publishModelCovariances(); 320 | 321 | // **** print diagnostics ******************************************* 322 | 323 | ros::WallTime end = ros::WallTime::now(); 324 | 325 | frame_count_++; 326 | 327 | int n_features = frame.keypoints.size(); 328 | int n_valid_features = frame.n_valid_keypoints; 329 | int n_model_pts = motion_estimation_.getModelSize(); 330 | 331 | double d_frame = 1000.0 * (end_frame - start_frame ).toSec(); 332 | double d_features = 1000.0 * (end_features - start_features).toSec(); 333 | double d_reg = 1000.0 * (end_reg - start_reg ).toSec(); 334 | double d_total = 1000.0 * (end - start ).toSec(); 335 | 336 | diagnostics(n_features, n_valid_features, n_model_pts, 337 | d_frame, d_features, d_reg, d_total); 338 | } 339 | 340 | void VisualOdometry::publishTf(const std_msgs::Header& header) 341 | { 342 | tf::StampedTransform transform_msg( 343 | f2b_, header.stamp, fixed_frame_, base_frame_); 344 | tf_broadcaster_.sendTransform (transform_msg); 345 | } 346 | 347 | void VisualOdometry::publishOdom(const std_msgs::Header& header) 348 | { 349 | OdomMsg odom; 350 | odom.header.stamp = header.stamp; 351 | odom.header.frame_id = fixed_frame_; 352 | tf::poseTFToMsg(f2b_, odom.pose.pose); 353 | odom_publisher_.publish(odom); 354 | } 355 | 356 | void VisualOdometry::publishPoseStamped(const std_msgs::Header& header) 357 | { 358 | geometry_msgs::PoseStamped::Ptr pose_stamped_msg; 359 | pose_stamped_msg = boost::make_shared(); 360 | pose_stamped_msg->header.stamp = header.stamp; 361 | pose_stamped_msg->header.frame_id = fixed_frame_; 362 | tf::poseTFToMsg(f2b_, pose_stamped_msg->pose); 363 | pose_stamped_publisher_.publish(pose_stamped_msg); 364 | } 365 | 366 | 367 | void VisualOdometry::publishPath(const std_msgs::Header& header) 368 | { 369 | path_msg_.header.stamp = header.stamp; 370 | path_msg_.header.frame_id = fixed_frame_; 371 | 372 | geometry_msgs::PoseStamped pose_stamped; 373 | pose_stamped.header.stamp = header.stamp; 374 | pose_stamped.header.frame_id = fixed_frame_; 375 | tf::poseTFToMsg(f2b_, pose_stamped.pose); 376 | 377 | path_msg_.poses.push_back(pose_stamped); 378 | path_pub_.publish(path_msg_); 379 | } 380 | 381 | bool VisualOdometry::getBaseToCameraTf(const std_msgs::Header& header) 382 | { 383 | tf::StampedTransform tf_m; 384 | 385 | try 386 | { 387 | tf_listener_.waitForTransform( 388 | base_frame_, header.frame_id, header.stamp, ros::Duration(1.0)); 389 | tf_listener_.lookupTransform ( 390 | base_frame_, header.frame_id, header.stamp, tf_m); 391 | } 392 | catch (tf::TransformException& ex) 393 | { 394 | ROS_WARN("Base to camera transform unavailable %s", ex.what()); 395 | return false; 396 | } 397 | 398 | b2c_ = tf_m; 399 | 400 | return true; 401 | } 402 | 403 | void VisualOdometry::gftReconfigCallback(GftDetectorConfig& config, uint32_t level) 404 | { 405 | rgbdtools::GftDetectorPtr gft_detector = 406 | boost::static_pointer_cast(feature_detector_); 407 | 408 | gft_detector->setNFeatures(config.n_features); 409 | gft_detector->setMinDistance(config.min_distance); 410 | } 411 | 412 | void VisualOdometry::starReconfigCallback(StarDetectorConfig& config, uint32_t level) 413 | { 414 | rgbdtools::StarDetectorPtr star_detector = 415 | boost::static_pointer_cast(feature_detector_); 416 | 417 | star_detector->setThreshold(config.threshold); 418 | star_detector->setMinDistance(config.min_distance); 419 | } 420 | 421 | void VisualOdometry::orbReconfigCallback(OrbDetectorConfig& config, uint32_t level) 422 | { 423 | rgbdtools::OrbDetectorPtr orb_detector = 424 | boost::static_pointer_cast(feature_detector_); 425 | 426 | orb_detector->setThreshold(config.threshold); 427 | orb_detector->setNFeatures(config.n_features); 428 | } 429 | 430 | void VisualOdometry::diagnostics( 431 | int n_features, int n_valid_features, int n_model_pts, 432 | double d_frame, double d_features, double d_reg, double d_total) 433 | { 434 | if(save_diagnostics_ && diagnostics_file_ != NULL) 435 | { 436 | // print to file 437 | fprintf(diagnostics_file_, "%d, %2.1f, %d, %3.1f, %d, %4.1f, %4.1f\n", 438 | frame_count_, 439 | d_frame, 440 | n_valid_features, d_features, 441 | n_model_pts, d_reg, 442 | d_total); 443 | } 444 | if (verbose_) 445 | { 446 | // print to screen 447 | ROS_INFO("[VO %d] %s[%d]: %.1f Reg[%d]: %.1f TOT: %.1f\n", 448 | frame_count_, 449 | detector_type_.c_str(), n_valid_features, d_features, 450 | n_model_pts, d_reg, 451 | d_total); 452 | } 453 | 454 | return; 455 | } 456 | 457 | void VisualOdometry::publishFeatureCloud(rgbdtools::RGBDFrame& frame) 458 | { 459 | PointCloudFeature feature_cloud; 460 | frame.constructFeaturePointCloud(feature_cloud); 461 | feature_cloud_publisher_.publish(feature_cloud); 462 | } 463 | 464 | void VisualOdometry::publishFeatureCovariances(rgbdtools::RGBDFrame& frame) 465 | { 466 | ///< @TODO publish feature covariances 467 | } 468 | 469 | void VisualOdometry::publishModelCloud() 470 | { 471 | PointCloudFeature::Ptr model_cloud_ptr = motion_estimation_.getModel(); 472 | model_cloud_ptr->header.frame_id = fixed_frame_; 473 | model_cloud_publisher_.publish(model_cloud_ptr); 474 | } 475 | 476 | void VisualOdometry::publishModelCovariances() 477 | { 478 | ///< @TODO ppublish model covariances 479 | } 480 | 481 | } // namespace ccny_rgbd 482 | -------------------------------------------------------------------------------- /ccny_rgbd/src/node/feature_viewer_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file feature_viewer_node.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "ccny_rgbd/apps/feature_viewer.h" 25 | 26 | int main(int argc, char** argv) 27 | { 28 | ros::init(argc, argv, "FeatureViewer"); 29 | ros::NodeHandle nh; 30 | ros::NodeHandle nh_private("~"); 31 | ccny_rgbd::FeatureViewer fv(nh, nh_private); 32 | ros::spin(); 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /ccny_rgbd/src/node/keyframe_mapper_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file keyframe_mapper_node.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "ccny_rgbd/apps/keyframe_mapper.h" 25 | 26 | int main(int argc, char** argv) 27 | { 28 | ros::init(argc, argv, "KeyframeMapper"); 29 | ros::NodeHandle nh; 30 | ros::NodeHandle nh_private("~"); 31 | ccny_rgbd::KeyframeMapper km(nh, nh_private); 32 | ros::spin(); 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /ccny_rgbd/src/node/rgbd_image_proc_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_image_proc.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "ccny_rgbd/apps/rgbd_image_proc.h" 25 | 26 | int main(int argc, char** argv) 27 | { 28 | ros::init(argc, argv, "RGBDImageProc"); 29 | ros::NodeHandle nh; 30 | ros::NodeHandle nh_private("~"); 31 | ccny_rgbd::RGBDImageProc rgbd_image_proc(nh, nh_private); 32 | ros::spin(); 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /ccny_rgbd/src/node/visual_odometry_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file visual_odometry_node.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "ccny_rgbd/apps/visual_odometry.h" 25 | 26 | int main(int argc, char** argv) 27 | { 28 | ros::init(argc, argv, "VisualOdometry"); 29 | ros::NodeHandle nh; 30 | ros::NodeHandle nh_private("~"); 31 | ccny_rgbd::VisualOdometry vo(nh, nh_private); 32 | ros::spin(); 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /ccny_rgbd/src/nodelet/rgbd_image_proc_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2013, City University of New York 3 | * Ivan Dryanovski 4 | * 5 | * CCNY Robotics Lab 6 | * http://robotics.ccny.cuny.edu 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | 22 | #include "ccny_rgbd/nodelet/rgbd_image_proc_nodelet.h" 23 | 24 | namespace ccny_rgbd { 25 | 26 | PLUGINLIB_DECLARE_CLASS(ccny_rgbd, RGBDImageProcNodelet, RGBDImageProcNodelet, nodelet::Nodelet); 27 | 28 | void RGBDImageProcNodelet::onInit() 29 | { 30 | NODELET_INFO("Initializing RGBD Image Proc Nodelet"); 31 | 32 | // TODO: Do we want the single threaded or multithreaded NH? 33 | ros::NodeHandle nh = getMTNodeHandle(); 34 | ros::NodeHandle nh_private = getMTPrivateNodeHandle(); 35 | 36 | rgbd_image_proc_ = new RGBDImageProc(nh, nh_private); 37 | } 38 | 39 | } // namespace ccny_rgbd -------------------------------------------------------------------------------- /ccny_rgbd/src/util.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file rgbd_util.cpp 3 | * @author Ivan Dryanovski 4 | * 5 | * @section LICENSE 6 | * 7 | * Copyright (C) 2013, City University of New York 8 | * CCNY Robotics Lab 9 | * 10 | * This program is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * This program is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with this program. If not, see . 22 | */ 23 | 24 | #include "ccny_rgbd/util.h" 25 | 26 | namespace ccny_rgbd { 27 | 28 | void getTfDifference(const tf::Transform& motion, double& dist, double& angle) 29 | { 30 | dist = motion.getOrigin().length(); 31 | double trace = motion.getBasis()[0][0] + motion.getBasis()[1][1] + motion.getBasis()[2][2]; 32 | angle = acos(std::min(1.0, std::max(-1.0, (trace - 1.0)/2.0))); 33 | } 34 | 35 | void getTfDifference(const tf::Transform& a, const tf::Transform b, double& dist, double& angle) 36 | { 37 | tf::Transform motion = a.inverse() * b; 38 | getTfDifference(motion, dist, angle); 39 | } 40 | 41 | tf::Transform tfFromEigen(Eigen::Matrix4f t) 42 | { 43 | tf::Transform tf; 44 | 45 | tf::Matrix3x3 m; 46 | m.setValue(t(0,0),t(0,1),t(0,2), 47 | t(1,0),t(1,1),t(1,2), 48 | t(2,0),t(2,1),t(2,2)); 49 | tf.setBasis(m); 50 | 51 | tf.setOrigin(tf::Vector3(t(0,3),t(1,3),t(2,3))); 52 | 53 | return tf; 54 | } 55 | 56 | Eigen::Matrix4f eigenFromTf(const tf::Transform& tf) 57 | { 58 | Eigen::Matrix4f out_mat; 59 | 60 | double mv[12]; 61 | tf.getBasis().getOpenGLSubMatrix(mv); 62 | 63 | tf::Vector3 origin = tf.getOrigin(); 64 | 65 | out_mat(0, 0) = mv[0]; out_mat(0, 1) = mv[4]; out_mat(0, 2) = mv[8]; 66 | out_mat(1, 0) = mv[1]; out_mat(1, 1) = mv[5]; out_mat(1, 2) = mv[9]; 67 | out_mat(2, 0) = mv[2]; out_mat(2, 1) = mv[6]; out_mat(2, 2) = mv[10]; 68 | 69 | out_mat(3, 0) = out_mat(3, 1) = out_mat(3, 2) = 0; out_mat(3, 3) = 1; 70 | out_mat(0, 3) = origin.x(); 71 | out_mat(1, 3) = origin.y(); 72 | out_mat(2, 3) = origin.z(); 73 | 74 | return out_mat; 75 | } 76 | 77 | 78 | tf::Transform tfFromEigenAffine(const AffineTransform& t) 79 | { 80 | tf::Transform tf; 81 | 82 | tf::Matrix3x3 m; 83 | m.setValue(t(0,0),t(0,1),t(0,2), 84 | t(1,0),t(1,1),t(1,2), 85 | t(2,0),t(2,1),t(2,2)); 86 | tf.setBasis(m); 87 | 88 | tf.setOrigin(tf::Vector3(t(0,3),t(1,3),t(2,3))); 89 | 90 | return tf; 91 | } 92 | 93 | AffineTransform eigenAffineFromTf(const tf::Transform& tf) 94 | { 95 | AffineTransform affine; 96 | 97 | double mv[12]; 98 | tf.getBasis().getOpenGLSubMatrix(mv); 99 | 100 | tf::Vector3 origin = tf.getOrigin(); 101 | 102 | affine(0, 0) = mv[0]; affine(0, 1) = mv[4]; affine (0, 2) = mv[8]; 103 | affine(1, 0) = mv[1]; affine(1, 1) = mv[5]; affine (1, 2) = mv[9]; 104 | affine(2, 0) = mv[2]; affine(2, 1) = mv[6]; affine (2, 2) = mv[10]; 105 | 106 | affine(3, 0) = affine(3, 1) = affine(3, 2) = 0; affine(3, 3) = 1; 107 | affine(0, 3) = origin.x(); 108 | affine(1, 3) = origin.y(); 109 | affine(2, 3) = origin.z(); 110 | 111 | return affine; 112 | } 113 | 114 | void tfToXYZRPY( 115 | const tf::Transform& t, 116 | double& x, double& y, double& z, 117 | double& roll, double& pitch, double& yaw) 118 | { 119 | x = t.getOrigin().getX(); 120 | y = t.getOrigin().getY(); 121 | z = t.getOrigin().getZ(); 122 | 123 | tf::Matrix3x3 m(t.getRotation()); 124 | m.getRPY(roll, pitch, yaw); 125 | } 126 | 127 | bool tfGreaterThan(const tf::Transform& tf, double dist, double angle) 128 | { 129 | double d = tf.getOrigin().length(); 130 | 131 | if (d >= dist) return true; 132 | 133 | double trace = tf.getBasis()[0][0] + tf.getBasis()[1][1] + tf.getBasis()[2][2]; 134 | double a = acos(std::min(1.0, std::max(-1.0, (trace - 1.0)/2.0))); 135 | 136 | if (a > angle) return true; 137 | 138 | return false; 139 | } 140 | 141 | double getMsDuration(const ros::WallTime& start) 142 | { 143 | return (ros::WallTime::now() - start).toSec() * 1000.0; 144 | } 145 | 146 | void removeInvalidMeans( 147 | const Vector3fVector& means, 148 | const BoolVector& valid, 149 | Vector3fVector& means_f) 150 | { 151 | unsigned int size = valid.size(); 152 | for(unsigned int i = 0; i < size; ++i) 153 | { 154 | if (valid[i]) 155 | { 156 | const Vector3f& mean = means[i]; 157 | means_f.push_back(mean); 158 | } 159 | } 160 | } 161 | 162 | void removeInvalidDistributions( 163 | const Vector3fVector& means, 164 | const Matrix3fVector& covariances, 165 | const BoolVector& valid, 166 | Vector3fVector& means_f, 167 | Matrix3fVector& covariances_f) 168 | { 169 | unsigned int size = valid.size(); 170 | for(unsigned int i = 0; i < size; ++i) 171 | { 172 | if (valid[i]) 173 | { 174 | const Vector3f& mean = means[i]; 175 | const Matrix3f& cov = covariances[i]; 176 | 177 | means_f.push_back(mean); 178 | covariances_f.push_back(cov); 179 | } 180 | } 181 | } 182 | 183 | void tfToEigenRt( 184 | const tf::Transform& tf, 185 | Matrix3f& R, 186 | Vector3f& t) 187 | { 188 | double mv[12]; 189 | tf.getBasis().getOpenGLSubMatrix(mv); 190 | 191 | tf::Vector3 origin = tf.getOrigin(); 192 | 193 | R(0, 0) = mv[0]; R(0, 1) = mv[4]; R(0, 2) = mv[8]; 194 | R(1, 0) = mv[1]; R(1, 1) = mv[5]; R(1, 2) = mv[9]; 195 | R(2, 0) = mv[2]; R(2, 1) = mv[6]; R(2, 2) = mv[10]; 196 | 197 | t(0, 0) = origin.x(); 198 | t(1, 0) = origin.y(); 199 | t(2, 0) = origin.z(); 200 | } 201 | 202 | void tfToOpenCVRt( 203 | const tf::Transform& transform, 204 | cv::Mat& R, 205 | cv::Mat& t) 206 | { 207 | // extract translation 208 | tf::Vector3 translation_tf = transform.getOrigin(); 209 | t = cv::Mat(3, 1, CV_64F); 210 | t.at(0,0) = translation_tf.getX(); 211 | t.at(1,0) = translation_tf.getY(); 212 | t.at(2,0) = translation_tf.getZ(); 213 | 214 | // extract rotation 215 | tf::Matrix3x3 rotation_tf(transform.getRotation()); 216 | R = cv::Mat(3, 3, CV_64F); 217 | for(int i = 0; i < 3; ++i) 218 | for(int j = 0; j < 3; ++j) 219 | R.at(j,i) = rotation_tf[j][i]; 220 | } 221 | 222 | void openCVRtToTf( 223 | const cv::Mat& R, 224 | const cv::Mat& t, 225 | tf::Transform& transform) 226 | { 227 | tf::Vector3 translation_tf( 228 | t.at(0,0), 229 | t.at(1,0), 230 | t.at(2,0)); 231 | 232 | tf::Matrix3x3 rotation_tf; 233 | for(int i = 0; i < 3; ++i) 234 | for(int j = 0; j < 3; ++j) 235 | rotation_tf[j][i] = R.at(j,i); 236 | 237 | transform.setOrigin(translation_tf); 238 | transform.setBasis(rotation_tf); 239 | } 240 | 241 | void convertCameraInfoToMats( 242 | const CameraInfoMsg::ConstPtr camera_info_msg, 243 | cv::Mat& intr, 244 | cv::Mat& dist) 245 | { 246 | // set intrinsic matrix from K vector 247 | intr = cv::Mat(3, 3, CV_64FC1); 248 | for (int idx = 0; idx < 9; ++idx) 249 | { 250 | int i = idx % 3; 251 | int j = idx / 3; 252 | intr.at(j, i) = camera_info_msg->K[idx]; 253 | } 254 | 255 | // set distortion matrix from D vector 256 | int d_size = camera_info_msg->D.size(); 257 | dist = cv::Mat(1, d_size, CV_64FC1); 258 | for (int idx = 0; idx < d_size; ++idx) 259 | { 260 | dist.at(0, idx) = camera_info_msg->D[idx]; 261 | } 262 | } 263 | 264 | void convertMatToCameraInfo( 265 | const cv::Mat& intr, 266 | CameraInfoMsg& camera_info_msg) 267 | { 268 | // set D matrix to 0 269 | camera_info_msg.D.resize(5); 270 | std::fill(camera_info_msg.D.begin(), camera_info_msg.D.end(), 0.0); 271 | 272 | // set K matrix to optimal new camera matrix 273 | for (int i = 0; i < 3; ++i) 274 | for (int j = 0; j < 3; ++j) 275 | camera_info_msg.K[j*3 + i] = intr.at(j,i); 276 | 277 | // set R matrix to identity 278 | std::fill(camera_info_msg.R.begin(), camera_info_msg.R.end(), 0.0); 279 | camera_info_msg.R[0*3 + 0] = 1.0; 280 | camera_info_msg.R[1*3 + 1] = 1.0; 281 | camera_info_msg.R[2*3 + 2] = 1.0; 282 | 283 | //set P matrix to K 284 | std::fill(camera_info_msg.P.begin(), camera_info_msg.P.end(), 0.0); 285 | for (int i = 0; i < 3; ++i) 286 | for (int j = 0; j < 3; ++j) 287 | camera_info_msg.P[j*4 + i] = intr.at(j,i); 288 | } 289 | 290 | void createRGBDFrameFromROSMessages( 291 | const ImageMsg::ConstPtr& rgb_msg, 292 | const ImageMsg::ConstPtr& depth_msg, 293 | const CameraInfoMsg::ConstPtr& info_msg, 294 | rgbdtools::RGBDFrame& frame) 295 | { 296 | // prepate opencv rgb image matrix 297 | cv::Mat rgb_img = cv_bridge::toCvShare(rgb_msg)->image; 298 | 299 | // prepate opencv depth image matrix 300 | // handles 16UC1 natively 301 | // 32FC1 need to be converted into 16UC1 302 | cv::Mat depth_img; 303 | 304 | const std::string& enc = depth_msg->encoding; 305 | if (enc.compare("16UC1") == 0) 306 | depth_img = cv_bridge::toCvShare(depth_msg)->image; 307 | else if (enc.compare("32FC1") == 0) 308 | rgbdtools::depthImageFloatTo16bit(cv_bridge::toCvShare(depth_msg)->image, depth_img); 309 | 310 | // prepare opencv intrinsic matrix from incoming camera info 311 | cv::Mat intr, dist; 312 | convertCameraInfoToMats(info_msg, intr, dist); 313 | /// @todo assert that distortion (dist) is 0 314 | 315 | // prepare rgbdtools header from incoming header 316 | rgbdtools::Header header; 317 | 318 | header.seq = rgb_msg->header.seq; 319 | header.frame_id = rgb_msg->header.frame_id; 320 | header.stamp.sec = rgb_msg->header.stamp.sec; 321 | header.stamp.nsec = rgb_msg->header.stamp.nsec; 322 | 323 | // initialize the RGBDframe 324 | frame = rgbdtools::RGBDFrame(rgb_img, depth_img, intr, header); 325 | } 326 | 327 | void pathEigenAffineToROS( 328 | const AffineTransformVector& path, 329 | PathMsg& path_msg) 330 | { 331 | assert(path.size() == path_msg.poses.size()); 332 | 333 | for (int idx = 0; idx < path.size(); ++idx) 334 | { 335 | tf::Transform pose_tf = tfFromEigenAffine(path[idx]); 336 | tf::poseTFToMsg(pose_tf, path_msg.poses[idx].pose); 337 | } 338 | } 339 | 340 | void pathROSToEigenAffine( 341 | const PathMsg& path_msg, 342 | AffineTransformVector& path) 343 | { 344 | path.clear(); 345 | path.resize(path_msg.poses.size()); 346 | 347 | for (int idx = 0; idx < path.size(); ++idx) 348 | { 349 | tf::Transform pose_tf; 350 | tf::poseMsgToTF(path_msg.poses[idx].pose, pose_tf); 351 | path[idx] = eigenAffineFromTf(pose_tf); 352 | } 353 | } 354 | 355 | } //namespace ccny_rgbd 356 | -------------------------------------------------------------------------------- /ccny_rgbd/srv/AddManualKeyframe.srv: -------------------------------------------------------------------------------- 1 | --- 2 | -------------------------------------------------------------------------------- /ccny_rgbd/srv/GenerateGraph.srv: -------------------------------------------------------------------------------- 1 | --- 2 | -------------------------------------------------------------------------------- /ccny_rgbd/srv/Load.srv: -------------------------------------------------------------------------------- 1 | string filename 2 | --- 3 | -------------------------------------------------------------------------------- /ccny_rgbd/srv/PublishKeyframe.srv: -------------------------------------------------------------------------------- 1 | int32 id 2 | --- 3 | -------------------------------------------------------------------------------- /ccny_rgbd/srv/PublishKeyframes.srv: -------------------------------------------------------------------------------- 1 | string re 2 | --- 3 | -------------------------------------------------------------------------------- /ccny_rgbd/srv/Save.srv: -------------------------------------------------------------------------------- 1 | string filename 2 | --- 3 | -------------------------------------------------------------------------------- /ccny_rgbd/srv/SolveGraph.srv: -------------------------------------------------------------------------------- 1 | --- 2 | -------------------------------------------------------------------------------- /ccny_rgbd_data/ROS_NOBUILD: -------------------------------------------------------------------------------- 1 | created by rosmake to mark as installed -------------------------------------------------------------------------------- /ccny_rgbd_data/calibration_asus_xtion_pro/depth.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | image_width: 640 3 | image_height: 480 4 | camera_name: depth 5 | camera_matrix: !!opencv-matrix 6 | rows: 3 7 | cols: 3 8 | dt: d 9 | data: [578.852799192617, 0., 316.003688477099, 0., 574.608347232532, 246.041146276871, 0., 0., 1] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: !!opencv-matrix 12 | rows: 1 13 | cols: 5 14 | dt: d 15 | data: [-0.0254191085871737, 0.000217207380690404, -0.000929267016847252, 0.000197345592394994, 0.] 16 | rectification_matrix: !!opencv-matrix 17 | rows: 3 18 | cols: 3 19 | dt: d 20 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 21 | projection_matrix: !!opencv-matrix 22 | rows: 3 23 | cols: 4 24 | dt: d 25 | data: [573.41162109375, 0, 315.553713880334, 0, 0, 570.878601074219, 245.299279804522, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /ccny_rgbd_data/calibration_asus_xtion_pro/extr.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | ir2rgb: !!opencv-matrix 3 | rows: 3 4 | cols: 4 5 | dt: d 6 | data: [ 9.9996455464580047e-01, -3.8543662740507505e-03, 7 | -7.4855402377836736e-03, -2.6010023400690397e+01, 8 | 3.8172756389720274e-03, 9.9998039855718457e-01, 9 | -4.9629535672246700e-03, -5.3322955648595599e-02, 10 | 7.5045225512439511e-03, 4.9342032831833664e-03, 11 | 9.9995966707624673e-01, 5.6801588171326900e-01 ] 12 | -------------------------------------------------------------------------------- /ccny_rgbd_data/calibration_asus_xtion_pro/rgb.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | image_width: 640 3 | image_height: 480 4 | camera_name: rgb 5 | camera_matrix: !!opencv-matrix 6 | rows: 3 7 | cols: 3 8 | dt: d 9 | data: [537.673722507338, 0., 321.226061527052, 0., 534.380205679756, 249.773992466202, 0., 0., 1.] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: !!opencv-matrix 12 | rows: 1 13 | cols: 5 14 | dt: d 15 | data: [0.0395956005042652, -0.111310452999064, 0.00211988964071199, 0.00070924348636878, 0.] 16 | rectification_matrix: !!opencv-matrix 17 | rows: 3 18 | cols: 3 19 | dt: d 20 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 21 | projection_matrix: !!opencv-matrix 22 | rows: 3 23 | cols: 4 24 | dt: d 25 | data: [536.882873535156, 0, 321.181075785589, 0, 0, 535.01953125, 249.981795385829, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /ccny_rgbd_data/calibration_openni_default/depth.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | image_width: 640 3 | image_height: 480 4 | camera_name: depth 5 | camera_matrix: !!opencv-matrix 6 | rows: 3 7 | cols: 3 8 | dt: d 9 | data: [570.3422241210938, 0.0, 314.5, 0.0, 570.3422241210938, 235.5, 0.0, 0.0, 1.0] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: !!opencv-matrix 12 | rows: 1 13 | cols: 5 14 | dt: d 15 | data: [0.0, 0.0, 0.0, 0.0, 0.0] 16 | rectification_matrix: !!opencv-matrix 17 | rows: 3 18 | cols: 3 19 | dt: d 20 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 21 | projection_matrix: !!opencv-matrix 22 | rows: 3 23 | cols: 4 24 | dt: d 25 | data: [570.3422241210938, 0.0, 314.5, 0.0, 0.0, 570.3422241210938, 235.5, 0.0, 0.0, 0.0, 1.0, 0.0] -------------------------------------------------------------------------------- /ccny_rgbd_data/calibration_openni_default/extr.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | ir2rgb: !!opencv-matrix 3 | rows: 3 4 | cols: 4 5 | dt: d 6 | data: [ 1.0, 0.0, 0.0, -20.0, 7 | 0.0, 1.0, 0.0, 0.0, 8 | 0.0, 0.0, 1.0, 0.0] 9 | -------------------------------------------------------------------------------- /ccny_rgbd_data/calibration_openni_default/rgb.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | image_width: 640 3 | image_height: 480 4 | camera_name: rgb 5 | camera_matrix: !!opencv-matrix 6 | rows: 3 7 | cols: 3 8 | dt: d 9 | data: [525.0, 0.0, 319.5, 0.0, 525.0, 239.5, 0.0, 0.0, 1.0] 10 | distortion_model: plumb_bob 11 | distortion_coefficients: !!opencv-matrix 12 | rows: 1 13 | cols: 5 14 | dt: d 15 | data: [0.0, 0.0, 0.0, 0.0, 0.0] 16 | rectification_matrix: !!opencv-matrix 17 | rows: 3 18 | cols: 3 19 | dt: d 20 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 21 | projection_matrix: !!opencv-matrix 22 | rows: 3 23 | cols: 4 24 | dt: d 25 | data: [525.0, 0.0, 319.5, 0.0, 0.0, 525.0, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0] -------------------------------------------------------------------------------- /ccny_rgbd_data/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Auxiliary data files for ccny_rgbd: calibration files, test sequences, etc. 5 | 6 | 7 | Ivan Dryanovski 8 | GPL 9 | 10 | http://ros.org/wiki/ccny_rgbd_data 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /lib_rgbdtools/Makefile: -------------------------------------------------------------------------------- 1 | all: rgbdtools_local 2 | 3 | GIT_DIR = rgbdtools_git 4 | BUILD_DIR = $(GIT_DIR)/build 5 | TOP_DIR = $(PWD) 6 | 7 | GIT_URL = https://github.com/ccny-ros-pkg/rgbdtools.git 8 | #GIT_REVISION = master 9 | 10 | include $(shell rospack find mk)/git_checkout.mk 11 | 12 | rgbdtools_local: $(GIT_DIR) 13 | mkdir -p $(BUILD_DIR) 14 | cd $(BUILD_DIR) && cmake ../ -DCMAKE_INSTALL_PREFIX=$(TOP_DIR) 15 | cd $(BUILD_DIR) && make install 16 | touch rgbdtools 17 | 18 | clean: 19 | cd $(BUILD_DIR) && make clean 20 | rm -rf rgbdtools rospack_nosubdirs 21 | 22 | wipe: clean 23 | -rm -rf $(GIT_DIR) include lib bin rospack_nosubdirs 24 | -------------------------------------------------------------------------------- /lib_rgbdtools/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Local install of rgbdtools library. 5 | 6 | 7 | Ivan Dryanovski 8 | GPL 9 | 10 | http://ros.org/wiki/lib_rgbdtools 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /stack.xml: -------------------------------------------------------------------------------- 1 | 2 | A collection of tools for working with RGB-D camera data developed at the CCNY Robotics Lab 3 | Maintained by Ivan Dryanovski 4 | GPL, LGPLv3 5 | 6 | http://ros.org/wiki/ccny_rgbd_tools 7 | 0.1.0 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | --------------------------------------------------------------------------------