├── CMakeLists.txt ├── README.md ├── ReadMe_and_supplementary_materials ├── LICENSE ├── Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne.html ├── Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files │ ├── 14936902.png │ ├── 14936902_002.png │ ├── 16076150.jpeg │ ├── 16076150_002.jpeg │ ├── 1f389.png │ ├── 1f44d.png │ ├── 1f44e.png │ ├── 1f604.png │ ├── 1f615.png │ ├── 2180161.jpeg │ ├── 2180161_002.jpeg │ ├── 2764.png │ ├── 4560248.png │ ├── 4560248_002.png │ ├── frameworks-3c1694fab1568340f2e75b26efa1f55d97c5153364a7357e9.css │ ├── frameworks-851de55546d87ad786f3efef3b5634f8f8a40d67fa6d44f854.js │ ├── github-3e95d73eb454e0099947df00d91ab0dbfc6b10be69dd5daf5de7a.css │ ├── github-9a6de3c5d3dd93cd9d6638a6d1bcc27eb7cbf9afdd082771ea900f.js │ └── octocat-spinner-32.gif ├── README.md ├── ReadMe.my ├── ReadMe.txt ├── ReadMe.txt~ ├── ReadMe~ ├── loam_velodyne - ROS Wiki.html ├── loam_velodyne - ROS Wiki_files │ ├── bootstrap.css │ ├── bootstrap.js │ ├── brought_by_horiz.png │ ├── common.css │ ├── common.js │ ├── ga.js │ ├── jquery-1.js │ ├── jquery.js │ ├── loam_velodyne.jpeg │ ├── loam_velodyne_002.jpeg │ ├── loam_velodyne_003.jpeg │ ├── loam_velodyne_004.jpeg │ ├── menu_browse_software.png │ ├── menu_documentation.png │ ├── menu_download.png │ ├── menu_left.png │ ├── menu_news.png │ ├── menu_right.png │ ├── menu_spacer.png │ ├── print.css │ ├── projection.css │ ├── ros_org.png │ ├── rosversion.js │ ├── roswiki.js │ ├── screen.css │ ├── seesaw.js │ └── sorttable.js └── original_src │ ├── laserMapping.cpp │ ├── laserMapping.cpp~ │ ├── laserOdometry.cpp │ ├── laserOdometry.cpp~ │ ├── scanRegistration.cpp │ ├── scanRegistration.cpp~ │ ├── transformMaintenance.cpp │ └── transformMaintenance.cpp~ ├── include └── loam_velodyne │ └── common.h ├── launch ├── hector_loam_velodyne.launch └── loam_velodyne.launch ├── package.xml ├── rviz_cfg └── loam_velodyne.rviz └── src ├── laserMapping.cpp ├── laserMapping_c.cpp ├── laserOdometry.cpp ├── scanRegistration.cpp ├── transformMaintenance.cpp └── transformMaintenance_c.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(loam_velodyne) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | nav_msgs 7 | sensor_msgs 8 | roscpp 9 | rospy 10 | std_msgs 11 | tf) 12 | 13 | find_package(Eigen3 REQUIRED) 14 | find_package(PCL REQUIRED) 15 | find_package(OpenCV REQUIRED) 16 | 17 | include_directories( 18 | include 19 | ${catkin_INCLUDE_DIRS} 20 | ${EIGEN3_INCLUDE_DIR} 21 | ${PCL_INCLUDE_DIRS}) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 25 | DEPENDS EIGEN3 PCL OpenCV 26 | INCLUDE_DIRS include 27 | ) 28 | 29 | 30 | add_executable(scanRegistration src/scanRegistration.cpp) 31 | target_link_libraries(scanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 32 | 33 | add_executable(laserOdometry src/laserOdometry.cpp) 34 | target_link_libraries(laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 35 | 36 | add_executable(laserMapping src/laserMapping.cpp) 37 | target_link_libraries(laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 38 | 39 | add_executable(transformMaintenance src/transformMaintenance.cpp) 40 | target_link_libraries(transformMaintenance ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) 41 | 42 | if (CATKIN_ENABLE_TESTING) 43 | find_package(rostest REQUIRED) 44 | # TODO: Download test data 45 | catkin_download_test_data(${PROJECT_NAME}_test_data.tar.gz 46 | https://dl.dropboxusercontent.com/s/y4hn486461tfmpm/velodyne_loam_test_data.tar.gz 47 | MD5 3d5194e6981975588b7a93caebf79ba4) 48 | add_custom_target(${PROJECT_NAME}_test_data 49 | COMMAND ${CMAKE_COMMAND} -E tar -xzf velodyne_loam_test_data.tar.gz 50 | DEPENDS ${PROJECT_NAME}_test_data.tar.gz) 51 | configure_file(tests/loam.test.in 52 | ${PROJECT_BINARY_DIR}/test/loam.test) 53 | add_rostest(${PROJECT_BINARY_DIR}/test/loam.test 54 | DEPENDENCIES 55 | ${PROJECT_NAME}_test_data 56 | scanRegistration 57 | laserOdometry 58 | laserMapping 59 | transformMaintenance) 60 | endif() 61 | 62 | 63 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # loam_velodyne 2 | 3 | This is a LOAM (Lidar Odometry and Mapping) ROS package for Velodyne VLP-16 3D laser scanner. This package is a simple modified copy of [loam_velodyne git repository](https://github.com/laboshinl/loam_velodyne) from **laboshinl**, which is again a modified copy of the original one release by [Ji Zhang](http://www.frc.ri.cmu.edu/~jizhang03/). His change on top of the original one is that he changed the scanRegistration.cpp to make it work with his dataset. I fixed a bug on laserOdometry.cpp to get rid of the matrix NaN error during L-M optimization step. Please cite Zhang et al.'s paper if this package is used. 4 | 5 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.([PDF](http://www.frc.ri.cmu.edu/~jizhang03/Publications/RSS_2014.pdf))([VIDEO](https://www.youtube.com/watch?feature=player_embedded&v=8ezyhTAEyHs))([SRC FILES](http://docs.ros.org/indigo/api/loam_velodyne/html/files.html)) 6 | 7 | Wiki Webpage by the Author: [http://wiki.ros.org/loam_velodyne](http://wiki.ros.org/loam_velodyne) 8 | 9 | LOAM back and forth 10 | 11 | # how to use 12 | Here I assume you have a Catkin worksapce under ~/ros_workspace/catkin_ws. 13 | 14 | (1) gitclone the package into your "src" folder. 15 | + **cd ~/ros_workspace/catkin_ws/src** 16 | + **git clone https://github.com/daobilige-su/loam_velodyne** 17 | 18 | (2) compile the package 19 | + **cd ~/ros_workspace/catkin_ws** 20 | + **catkin_make** 21 | 22 | (3) download a ROS bag file for test dataset. 23 | + **roscd loam_velodyne** 24 | + **mkdir data** 25 | + download [nsh_indoor_outdoor](www.frc.ri.cmu.edu/~jizhang03/Datasets/nsh_indoor_outdoor.bag), [gates_oscillating_motion](www.frc.ri.cmu.edu/~jizhang03/Datasets/gates_oscillating_motion.bag) and [laboshinl VLP-16](https://db.tt/t2r39mjZ)(unzip it) dataset into your ~/ros_workspace/catkin_ws/src/loam_velodyne/data/ folder. 26 | 27 | (4) run the package and rosbag file 28 | + 1) in 1st terminal: 29 | + **roslaunch loam_velodyne loam_velodyne.launch** 30 | + 2) in 2nd terminal: 31 | + **roscd loam_velodyne/data/** 32 | + **rosbay play nsh_indoor_outdoor.bag** or **rosbay play gates_oscillating_motion.bag** or **rosbay play 2016-04-11-13-24-42.bag**(for laboshinl VLP-16 dataset) 33 | + (use -r 0.5 if the result look bad and it is due to the less powerful CPU, (e.g. rosbay play nsh_indoor_outdoor.bag -r 0.5)) 34 | 35 | YOU SHOULD SEE A RESULT SIMILAR TO THEIR DEMO VIDEO ([nsh_indoor_outdoor DEMO VIDEO](http://www.frc.ri.cmu.edu/~jizhang03/Videos/nsh_indoor_outdoor.mp4), [gates_oscillating_motion DEMO VIDEO](http://www.frc.ri.cmu.edu/~jizhang03/Videos/gates_oscillating_motion.mp4)) and [laboshinl VLP-16 DEMO VIDEO](https://www.youtube.com/watch?v=o1cLXY-Es54&feature=youtu.be). GOOD LUCK. 36 | 37 | # Known Issue: 38 | After two modifications (**laboshinl** and I), the algorithm starts diverging at some point when "gates_oscillating_motion" dataset is being processed. Needs parameters tuning? 39 | 40 | # Original Introduction by the author: 41 | 42 | Laser Odometry and Mapping (Loam) is a realtime method for state estimation and mapping using a 3D lidar. The program contains two major threads running in parallel. An "odometry" thread computes motion of the lidar between two sweeps, at a higher frame rate. It also removes distortion in the point cloud caused by motion of the lidar. A "mapping" thread takes the undistorted point cloud and incrementally builds a map, while simultaneously computes pose of the lidar on the map at a lower frame rate. The lidar state estimation is combination of the outputs from the two threads. 43 | 44 | If an IMU is available, the orientation (integrated from angular rate) and acceleration measurements are used to deal with general motion of the lidar, while the program takes care of the linear motion. 45 | 46 | The program is tested on a laptop with 2.5 GHz quad cores and 6 Gib memory (the program consumes two cores). It uses a Velodyne VLP-16 lidar. 47 | 48 | Wiki Webpage: http://wiki.ros.org/loam_velodyne 49 | 50 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2013, Ji Zhang, Carnegie Mellon University 2 | Further contributions copyright (c) 2016, Southwest Research Institute 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 3. Neither the name of the copyright holder nor the names of its contributors 14 | may be used to endorse or promote products derived from this software without 15 | specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,OR 25 | TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | 28 | This is an implementation of the algorithm described in the following paper: 29 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 30 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/14936902.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/14936902.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/14936902_002.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/14936902_002.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/16076150.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/16076150.jpeg -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/16076150_002.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/16076150_002.jpeg -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/1f389.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/1f389.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/1f44d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/1f44d.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/1f44e.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/1f44e.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/1f604.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/1f604.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/1f615.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/1f615.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/2180161.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/2180161.jpeg -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/2180161_002.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/2180161_002.jpeg -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/2764.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/2764.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/4560248.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/4560248.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/4560248_002.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/4560248_002.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/octocat-spinner-32.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/Public place to ask questions, post and discuss issues · Issue #3 · laboshinl_loam_velodyne_files/octocat-spinner-32.gif -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/README.md: -------------------------------------------------------------------------------- 1 | :white_check_mark: Tested with ROS Indigo and Velodyne VLP16. [(Screencast)](https://youtu.be/o1cLXY-Es54) 2 | 3 | All sources were taken from [ROS documentation](http://docs.ros.org/indigo/api/loam_velodyne/html/files.html) 4 | 5 | Ask questions [here](https://github.com/laboshinl/loam_velodyne/issues/3). 6 | 7 | 8 | How to build with catkin: 9 | 10 | ``` 11 | $ cd ~/catkin_ws/src/ 12 | $ git clone https://github.com/laboshinl/loam_velodyne.git 13 | $ cd ~/catkin_ws 14 | $ catkin_make -DCMAKE_BUILD_TYPE=Release 15 | $ source ~/catkin_ws/devel/setup.bash 16 | ``` 17 | 18 | Running: 19 | ``` 20 | roslaunch ~/catkin_ws/src/loam_velodyne/launch/loam_velodyne.launch 21 | ``` 22 | 23 | In second terminal play sample velodyne data from [VLP16 rosbag](https://db.tt/t2r39mjZ): 24 | ``` 25 | rosbag play ~/Downloads/velodyne.bag 26 | ``` 27 | 28 | Or read from velodyne [VLP16 sample pcap](https://midas3.kitware.com/midas/folder/12979): 29 | ``` 30 | roslaunch velodyne_pointcloud VLP16_points.launch pcap:="/home/laboshinl/Downloads/velodyne.pcap" 31 | ``` 32 | 33 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/ReadMe.my: -------------------------------------------------------------------------------- 1 | For information about his package, please refer to: 2 | "ReadMe_and_supplementary_materials" 3 | folder 4 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/ReadMe.txt: -------------------------------------------------------------------------------- 1 | This package is NOT the original package implemented by Ji Zhang. 2 | The original files from Ji Zhang can be found on ROS package documentation archive (http://docs.ros.org/indigo/api/loam_velodyne/html/files.html). Also attached in this folder (old_src) folder. 3 | some example videos can be found: 4 | https://www.youtube.com/watch?v=8ezyhTAEyHs#t=155 5 | https://www.youtube.com/watch?v=ld2lO5mnjO8 6 | https://plus.google.com/+JiZhang_CMU/posts 7 | 8 | This package is the modified version of the original by laboshinl. 9 | (https://github.com/laboshinl) 10 | It doesnot work well with "nsh_indoor_outdoor.bag" data or "play gates_oscillating_motion.bag" data. The main change by laboshinl is 11 | -- 12 | I've created package and launch file. I've also modified scanRegistration.cpp to get it to work with my VLP16 (I think originally it was for HDL32 ) 13 | -- 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/ReadMe.txt~: -------------------------------------------------------------------------------- 1 | This package is NOT the original package implemented by Ji Zhang. 2 | The original files from Ji Zhang can be found on ROS package documentation archive (http://docs.ros.org/indigo/api/loam_velodyne/html/files.html). Also attached in this folder (old_src) folder. 3 | 4 | This package is the modified version of the original by laboshinl. 5 | (https://github.com/laboshinl) 6 | It doesnot work well with "nsh_indoor_outdoor.bag" data or "play gates_oscillating_motion.bag" data. The main change by laboshinl is 7 | -- 8 | I've created package and launch file. I've also modified scanRegistration.cpp to get it to work with my VLP16 (I think originally it was for HDL32 ) 9 | -- 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/ReadMe~: -------------------------------------------------------------------------------- 1 | This package is NOT the original package implemented by Ji Zhang. 2 | The original files from Ji Zhang can be found on ROS package documentation archive (http://docs.ros.org/indigo/api/loam_velodyne/html/files.html). Also attached in this folder (old_src) folder. 3 | 4 | This package is the modified version of the original by laboshinl. 5 | (https://github.com/laboshinl) 6 | It doesnot work well with "nsh_indoor_outdoor.bag" data or "play gates_oscillating_motion.bag" data. The main change by laboshinl is 7 | -- 8 | I've created package and launch file. I've also modified scanRegistration.cpp to get it to work with my VLP16 (I think originally it was for HDL32 ) 9 | -- 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/bootstrap.js: -------------------------------------------------------------------------------- 1 | /*! 2 | * Bootstrap v3.1.1 (http://getbootstrap.com) 3 | * Copyright 2011-2014 Twitter, Inc. 4 | * Licensed under MIT (https://github.com/twbs/bootstrap/blob/master/LICENSE) 5 | */ 6 | if("undefined"==typeof jQuery)throw new Error("Bootstrap's JavaScript requires jQuery");+function(a){"use strict";function b(){var a=document.createElement("bootstrap"),b={WebkitTransition:"webkitTransitionEnd",MozTransition:"transitionend",OTransition:"oTransitionEnd otransitionend",transition:"transitionend"};for(var c in b)if(void 0!==a.style[c])return{end:b[c]};return!1}a.fn.emulateTransitionEnd=function(b){var c=!1,d=this;a(this).one(a.support.transition.end,function(){c=!0});var e=function(){c||a(d).trigger(a.support.transition.end)};return setTimeout(e,b),this},a(function(){a.support.transition=b()})}(jQuery),+function(a){"use strict";var b='[data-dismiss="alert"]',c=function(c){a(c).on("click",b,this.close)};c.prototype.close=function(b){function c(){f.trigger("closed.bs.alert").remove()}var d=a(this),e=d.attr("data-target");e||(e=d.attr("href"),e=e&&e.replace(/.*(?=#[^\s]*$)/,""));var f=a(e);b&&b.preventDefault(),f.length||(f=d.hasClass("alert")?d:d.parent()),f.trigger(b=a.Event("close.bs.alert")),b.isDefaultPrevented()||(f.removeClass("in"),a.support.transition&&f.hasClass("fade")?f.one(a.support.transition.end,c).emulateTransitionEnd(150):c())};var d=a.fn.alert;a.fn.alert=function(b){return this.each(function(){var d=a(this),e=d.data("bs.alert");e||d.data("bs.alert",e=new c(this)),"string"==typeof b&&e[b].call(d)})},a.fn.alert.Constructor=c,a.fn.alert.noConflict=function(){return a.fn.alert=d,this},a(document).on("click.bs.alert.data-api",b,c.prototype.close)}(jQuery),+function(a){"use strict";var b=function(c,d){this.$element=a(c),this.options=a.extend({},b.DEFAULTS,d),this.isLoading=!1};b.DEFAULTS={loadingText:"loading..."},b.prototype.setState=function(b){var c="disabled",d=this.$element,e=d.is("input")?"val":"html",f=d.data();b+="Text",f.resetText||d.data("resetText",d[e]()),d[e](f[b]||this.options[b]),setTimeout(a.proxy(function(){"loadingText"==b?(this.isLoading=!0,d.addClass(c).attr(c,c)):this.isLoading&&(this.isLoading=!1,d.removeClass(c).removeAttr(c))},this),0)},b.prototype.toggle=function(){var a=!0,b=this.$element.closest('[data-toggle="buttons"]');if(b.length){var c=this.$element.find("input");"radio"==c.prop("type")&&(c.prop("checked")&&this.$element.hasClass("active")?a=!1:b.find(".active").removeClass("active")),a&&c.prop("checked",!this.$element.hasClass("active")).trigger("change")}a&&this.$element.toggleClass("active")};var c=a.fn.button;a.fn.button=function(c){return this.each(function(){var d=a(this),e=d.data("bs.button"),f="object"==typeof c&&c;e||d.data("bs.button",e=new b(this,f)),"toggle"==c?e.toggle():c&&e.setState(c)})},a.fn.button.Constructor=b,a.fn.button.noConflict=function(){return a.fn.button=c,this},a(document).on("click.bs.button.data-api","[data-toggle^=button]",function(b){var c=a(b.target);c.hasClass("btn")||(c=c.closest(".btn")),c.button("toggle"),b.preventDefault()})}(jQuery),+function(a){"use strict";var b=function(b,c){this.$element=a(b),this.$indicators=this.$element.find(".carousel-indicators"),this.options=c,this.paused=this.sliding=this.interval=this.$active=this.$items=null,"hover"==this.options.pause&&this.$element.on("mouseenter",a.proxy(this.pause,this)).on("mouseleave",a.proxy(this.cycle,this))};b.DEFAULTS={interval:5e3,pause:"hover",wrap:!0},b.prototype.cycle=function(b){return b||(this.paused=!1),this.interval&&clearInterval(this.interval),this.options.interval&&!this.paused&&(this.interval=setInterval(a.proxy(this.next,this),this.options.interval)),this},b.prototype.getActiveIndex=function(){return this.$active=this.$element.find(".item.active"),this.$items=this.$active.parent().children(),this.$items.index(this.$active)},b.prototype.to=function(b){var c=this,d=this.getActiveIndex();return b>this.$items.length-1||0>b?void 0:this.sliding?this.$element.one("slid.bs.carousel",function(){c.to(b)}):d==b?this.pause().cycle():this.slide(b>d?"next":"prev",a(this.$items[b]))},b.prototype.pause=function(b){return b||(this.paused=!0),this.$element.find(".next, .prev").length&&a.support.transition&&(this.$element.trigger(a.support.transition.end),this.cycle(!0)),this.interval=clearInterval(this.interval),this},b.prototype.next=function(){return this.sliding?void 0:this.slide("next")},b.prototype.prev=function(){return this.sliding?void 0:this.slide("prev")},b.prototype.slide=function(b,c){var d=this.$element.find(".item.active"),e=c||d[b](),f=this.interval,g="next"==b?"left":"right",h="next"==b?"first":"last",i=this;if(!e.length){if(!this.options.wrap)return;e=this.$element.find(".item")[h]()}if(e.hasClass("active"))return this.sliding=!1;var j=a.Event("slide.bs.carousel",{relatedTarget:e[0],direction:g});return this.$element.trigger(j),j.isDefaultPrevented()?void 0:(this.sliding=!0,f&&this.pause(),this.$indicators.length&&(this.$indicators.find(".active").removeClass("active"),this.$element.one("slid.bs.carousel",function(){var b=a(i.$indicators.children()[i.getActiveIndex()]);b&&b.addClass("active")})),a.support.transition&&this.$element.hasClass("slide")?(e.addClass(b),e[0].offsetWidth,d.addClass(g),e.addClass(g),d.one(a.support.transition.end,function(){e.removeClass([b,g].join(" ")).addClass("active"),d.removeClass(["active",g].join(" ")),i.sliding=!1,setTimeout(function(){i.$element.trigger("slid.bs.carousel")},0)}).emulateTransitionEnd(1e3*d.css("transition-duration").slice(0,-1))):(d.removeClass("active"),e.addClass("active"),this.sliding=!1,this.$element.trigger("slid.bs.carousel")),f&&this.cycle(),this)};var c=a.fn.carousel;a.fn.carousel=function(c){return this.each(function(){var d=a(this),e=d.data("bs.carousel"),f=a.extend({},b.DEFAULTS,d.data(),"object"==typeof c&&c),g="string"==typeof c?c:f.slide;e||d.data("bs.carousel",e=new b(this,f)),"number"==typeof c?e.to(c):g?e[g]():f.interval&&e.pause().cycle()})},a.fn.carousel.Constructor=b,a.fn.carousel.noConflict=function(){return a.fn.carousel=c,this},a(document).on("click.bs.carousel.data-api","[data-slide], [data-slide-to]",function(b){var c,d=a(this),e=a(d.attr("data-target")||(c=d.attr("href"))&&c.replace(/.*(?=#[^\s]+$)/,"")),f=a.extend({},e.data(),d.data()),g=d.attr("data-slide-to");g&&(f.interval=!1),e.carousel(f),(g=d.attr("data-slide-to"))&&e.data("bs.carousel").to(g),b.preventDefault()}),a(window).on("load",function(){a('[data-ride="carousel"]').each(function(){var b=a(this);b.carousel(b.data())})})}(jQuery),+function(a){"use strict";var b=function(c,d){this.$element=a(c),this.options=a.extend({},b.DEFAULTS,d),this.transitioning=null,this.options.parent&&(this.$parent=a(this.options.parent)),this.options.toggle&&this.toggle()};b.DEFAULTS={toggle:!0},b.prototype.dimension=function(){var a=this.$element.hasClass("width");return a?"width":"height"},b.prototype.show=function(){if(!this.transitioning&&!this.$element.hasClass("in")){var b=a.Event("show.bs.collapse");if(this.$element.trigger(b),!b.isDefaultPrevented()){var c=this.$parent&&this.$parent.find("> .panel > .in");if(c&&c.length){var d=c.data("bs.collapse");if(d&&d.transitioning)return;c.collapse("hide"),d||c.data("bs.collapse",null)}var e=this.dimension();this.$element.removeClass("collapse").addClass("collapsing")[e](0),this.transitioning=1;var f=function(){this.$element.removeClass("collapsing").addClass("collapse in")[e]("auto"),this.transitioning=0,this.$element.trigger("shown.bs.collapse")};if(!a.support.transition)return f.call(this);var g=a.camelCase(["scroll",e].join("-"));this.$element.one(a.support.transition.end,a.proxy(f,this)).emulateTransitionEnd(350)[e](this.$element[0][g])}}},b.prototype.hide=function(){if(!this.transitioning&&this.$element.hasClass("in")){var b=a.Event("hide.bs.collapse");if(this.$element.trigger(b),!b.isDefaultPrevented()){var c=this.dimension();this.$element[c](this.$element[c]())[0].offsetHeight,this.$element.addClass("collapsing").removeClass("collapse").removeClass("in"),this.transitioning=1;var d=function(){this.transitioning=0,this.$element.trigger("hidden.bs.collapse").removeClass("collapsing").addClass("collapse")};return a.support.transition?void this.$element[c](0).one(a.support.transition.end,a.proxy(d,this)).emulateTransitionEnd(350):d.call(this)}}},b.prototype.toggle=function(){this[this.$element.hasClass("in")?"hide":"show"]()};var c=a.fn.collapse;a.fn.collapse=function(c){return this.each(function(){var d=a(this),e=d.data("bs.collapse"),f=a.extend({},b.DEFAULTS,d.data(),"object"==typeof c&&c);!e&&f.toggle&&"show"==c&&(c=!c),e||d.data("bs.collapse",e=new b(this,f)),"string"==typeof c&&e[c]()})},a.fn.collapse.Constructor=b,a.fn.collapse.noConflict=function(){return a.fn.collapse=c,this},a(document).on("click.bs.collapse.data-api","[data-toggle=collapse]",function(b){var c,d=a(this),e=d.attr("data-target")||b.preventDefault()||(c=d.attr("href"))&&c.replace(/.*(?=#[^\s]+$)/,""),f=a(e),g=f.data("bs.collapse"),h=g?"toggle":d.data(),i=d.attr("data-parent"),j=i&&a(i);g&&g.transitioning||(j&&j.find('[data-toggle=collapse][data-parent="'+i+'"]').not(d).addClass("collapsed"),d[f.hasClass("in")?"addClass":"removeClass"]("collapsed")),f.collapse(h)})}(jQuery),+function(a){"use strict";function b(b){a(d).remove(),a(e).each(function(){var d=c(a(this)),e={relatedTarget:this};d.hasClass("open")&&(d.trigger(b=a.Event("hide.bs.dropdown",e)),b.isDefaultPrevented()||d.removeClass("open").trigger("hidden.bs.dropdown",e))})}function c(b){var c=b.attr("data-target");c||(c=b.attr("href"),c=c&&/#[A-Za-z]/.test(c)&&c.replace(/.*(?=#[^\s]*$)/,""));var d=c&&a(c);return d&&d.length?d:b.parent()}var d=".dropdown-backdrop",e="[data-toggle=dropdown]",f=function(b){a(b).on("click.bs.dropdown",this.toggle)};f.prototype.toggle=function(d){var e=a(this);if(!e.is(".disabled, :disabled")){var f=c(e),g=f.hasClass("open");if(b(),!g){"ontouchstart"in document.documentElement&&!f.closest(".navbar-nav").length&&a('
').insertAfter(a(this)).on("click",b);var h={relatedTarget:this};if(f.trigger(d=a.Event("show.bs.dropdown",h)),d.isDefaultPrevented())return;f.toggleClass("open").trigger("shown.bs.dropdown",h),e.focus()}return!1}},f.prototype.keydown=function(b){if(/(38|40|27)/.test(b.keyCode)){var d=a(this);if(b.preventDefault(),b.stopPropagation(),!d.is(".disabled, :disabled")){var f=c(d),g=f.hasClass("open");if(!g||g&&27==b.keyCode)return 27==b.which&&f.find(e).focus(),d.click();var h=" li:not(.divider):visible a",i=f.find("[role=menu]"+h+", [role=listbox]"+h);if(i.length){var j=i.index(i.filter(":focus"));38==b.keyCode&&j>0&&j--,40==b.keyCode&&j').appendTo(document.body),this.$element.on("click.dismiss.bs.modal",a.proxy(function(a){a.target===a.currentTarget&&("static"==this.options.backdrop?this.$element[0].focus.call(this.$element[0]):this.hide.call(this))},this)),d&&this.$backdrop[0].offsetWidth,this.$backdrop.addClass("in"),!b)return;d?this.$backdrop.one(a.support.transition.end,b).emulateTransitionEnd(150):b()}else!this.isShown&&this.$backdrop?(this.$backdrop.removeClass("in"),a.support.transition&&this.$element.hasClass("fade")?this.$backdrop.one(a.support.transition.end,b).emulateTransitionEnd(150):b()):b&&b()};var c=a.fn.modal;a.fn.modal=function(c,d){return this.each(function(){var e=a(this),f=e.data("bs.modal"),g=a.extend({},b.DEFAULTS,e.data(),"object"==typeof c&&c);f||e.data("bs.modal",f=new b(this,g)),"string"==typeof c?f[c](d):g.show&&f.show(d)})},a.fn.modal.Constructor=b,a.fn.modal.noConflict=function(){return a.fn.modal=c,this},a(document).on("click.bs.modal.data-api",'[data-toggle="modal"]',function(b){var c=a(this),d=c.attr("href"),e=a(c.attr("data-target")||d&&d.replace(/.*(?=#[^\s]+$)/,"")),f=e.data("bs.modal")?"toggle":a.extend({remote:!/#/.test(d)&&d},e.data(),c.data());c.is("a")&&b.preventDefault(),e.modal(f,this).one("hide",function(){c.is(":visible")&&c.focus()})}),a(document).on("show.bs.modal",".modal",function(){a(document.body).addClass("modal-open")}).on("hidden.bs.modal",".modal",function(){a(document.body).removeClass("modal-open")})}(jQuery),+function(a){"use strict";var b=function(a,b){this.type=this.options=this.enabled=this.timeout=this.hoverState=this.$element=null,this.init("tooltip",a,b)};b.DEFAULTS={animation:!0,placement:"top",selector:!1,template:'
',trigger:"hover focus",title:"",delay:0,html:!1,container:!1},b.prototype.init=function(b,c,d){this.enabled=!0,this.type=b,this.$element=a(c),this.options=this.getOptions(d);for(var e=this.options.trigger.split(" "),f=e.length;f--;){var g=e[f];if("click"==g)this.$element.on("click."+this.type,this.options.selector,a.proxy(this.toggle,this));else if("manual"!=g){var h="hover"==g?"mouseenter":"focusin",i="hover"==g?"mouseleave":"focusout";this.$element.on(h+"."+this.type,this.options.selector,a.proxy(this.enter,this)),this.$element.on(i+"."+this.type,this.options.selector,a.proxy(this.leave,this))}}this.options.selector?this._options=a.extend({},this.options,{trigger:"manual",selector:""}):this.fixTitle()},b.prototype.getDefaults=function(){return b.DEFAULTS},b.prototype.getOptions=function(b){return b=a.extend({},this.getDefaults(),this.$element.data(),b),b.delay&&"number"==typeof b.delay&&(b.delay={show:b.delay,hide:b.delay}),b},b.prototype.getDelegateOptions=function(){var b={},c=this.getDefaults();return this._options&&a.each(this._options,function(a,d){c[a]!=d&&(b[a]=d)}),b},b.prototype.enter=function(b){var c=b instanceof this.constructor?b:a(b.currentTarget)[this.type](this.getDelegateOptions()).data("bs."+this.type);return clearTimeout(c.timeout),c.hoverState="in",c.options.delay&&c.options.delay.show?void(c.timeout=setTimeout(function(){"in"==c.hoverState&&c.show()},c.options.delay.show)):c.show()},b.prototype.leave=function(b){var c=b instanceof this.constructor?b:a(b.currentTarget)[this.type](this.getDelegateOptions()).data("bs."+this.type);return clearTimeout(c.timeout),c.hoverState="out",c.options.delay&&c.options.delay.hide?void(c.timeout=setTimeout(function(){"out"==c.hoverState&&c.hide()},c.options.delay.hide)):c.hide()},b.prototype.show=function(){var b=a.Event("show.bs."+this.type);if(this.hasContent()&&this.enabled){if(this.$element.trigger(b),b.isDefaultPrevented())return;var c=this,d=this.tip();this.setContent(),this.options.animation&&d.addClass("fade");var e="function"==typeof this.options.placement?this.options.placement.call(this,d[0],this.$element[0]):this.options.placement,f=/\s?auto?\s?/i,g=f.test(e);g&&(e=e.replace(f,"")||"top"),d.detach().css({top:0,left:0,display:"block"}).addClass(e),this.options.container?d.appendTo(this.options.container):d.insertAfter(this.$element);var h=this.getPosition(),i=d[0].offsetWidth,j=d[0].offsetHeight;if(g){var k=this.$element.parent(),l=e,m=document.documentElement.scrollTop||document.body.scrollTop,n="body"==this.options.container?window.innerWidth:k.outerWidth(),o="body"==this.options.container?window.innerHeight:k.outerHeight(),p="body"==this.options.container?0:k.offset().left;e="bottom"==e&&h.top+h.height+j-m>o?"top":"top"==e&&h.top-m-j<0?"bottom":"right"==e&&h.right+i>n?"left":"left"==e&&h.left-i

'}),b.prototype=a.extend({},a.fn.tooltip.Constructor.prototype),b.prototype.constructor=b,b.prototype.getDefaults=function(){return b.DEFAULTS},b.prototype.setContent=function(){var a=this.tip(),b=this.getTitle(),c=this.getContent();a.find(".popover-title")[this.options.html?"html":"text"](b),a.find(".popover-content")[this.options.html?"string"==typeof c?"html":"append":"text"](c),a.removeClass("fade top bottom left right in"),a.find(".popover-title").html()||a.find(".popover-title").hide()},b.prototype.hasContent=function(){return this.getTitle()||this.getContent()},b.prototype.getContent=function(){var a=this.$element,b=this.options;return a.attr("data-content")||("function"==typeof b.content?b.content.call(a[0]):b.content)},b.prototype.arrow=function(){return this.$arrow=this.$arrow||this.tip().find(".arrow")},b.prototype.tip=function(){return this.$tip||(this.$tip=a(this.options.template)),this.$tip};var c=a.fn.popover;a.fn.popover=function(c){return this.each(function(){var d=a(this),e=d.data("bs.popover"),f="object"==typeof c&&c;(e||"destroy"!=c)&&(e||d.data("bs.popover",e=new b(this,f)),"string"==typeof c&&e[c]())})},a.fn.popover.Constructor=b,a.fn.popover.noConflict=function(){return a.fn.popover=c,this}}(jQuery),+function(a){"use strict";function b(c,d){var e,f=a.proxy(this.process,this);this.$element=a(a(c).is("body")?window:c),this.$body=a("body"),this.$scrollElement=this.$element.on("scroll.bs.scroll-spy.data-api",f),this.options=a.extend({},b.DEFAULTS,d),this.selector=(this.options.target||(e=a(c).attr("href"))&&e.replace(/.*(?=#[^\s]+$)/,"")||"")+" .nav li > a",this.offsets=a([]),this.targets=a([]),this.activeTarget=null,this.refresh(),this.process()}b.DEFAULTS={offset:10},b.prototype.refresh=function(){var b=this.$element[0]==window?"offset":"position";this.offsets=a([]),this.targets=a([]);{var c=this;this.$body.find(this.selector).map(function(){var d=a(this),e=d.data("target")||d.attr("href"),f=/^#./.test(e)&&a(e);return f&&f.length&&f.is(":visible")&&[[f[b]().top+(!a.isWindow(c.$scrollElement.get(0))&&c.$scrollElement.scrollTop()),e]]||null}).sort(function(a,b){return a[0]-b[0]}).each(function(){c.offsets.push(this[0]),c.targets.push(this[1])})}},b.prototype.process=function(){var a,b=this.$scrollElement.scrollTop()+this.options.offset,c=this.$scrollElement[0].scrollHeight||this.$body[0].scrollHeight,d=c-this.$scrollElement.height(),e=this.offsets,f=this.targets,g=this.activeTarget;if(b>=d)return g!=(a=f.last()[0])&&this.activate(a);if(g&&b<=e[0])return g!=(a=f[0])&&this.activate(a);for(a=e.length;a--;)g!=f[a]&&b>=e[a]&&(!e[a+1]||b<=e[a+1])&&this.activate(f[a])},b.prototype.activate=function(b){this.activeTarget=b,a(this.selector).parentsUntil(this.options.target,".active").removeClass("active");var c=this.selector+'[data-target="'+b+'"],'+this.selector+'[href="'+b+'"]',d=a(c).parents("li").addClass("active");d.parent(".dropdown-menu").length&&(d=d.closest("li.dropdown").addClass("active")),d.trigger("activate.bs.scrollspy")};var c=a.fn.scrollspy;a.fn.scrollspy=function(c){return this.each(function(){var d=a(this),e=d.data("bs.scrollspy"),f="object"==typeof c&&c;e||d.data("bs.scrollspy",e=new b(this,f)),"string"==typeof c&&e[c]()})},a.fn.scrollspy.Constructor=b,a.fn.scrollspy.noConflict=function(){return a.fn.scrollspy=c,this},a(window).on("load",function(){a('[data-spy="scroll"]').each(function(){var b=a(this);b.scrollspy(b.data())})})}(jQuery),+function(a){"use strict";var b=function(b){this.element=a(b)};b.prototype.show=function(){var b=this.element,c=b.closest("ul:not(.dropdown-menu)"),d=b.data("target");if(d||(d=b.attr("href"),d=d&&d.replace(/.*(?=#[^\s]*$)/,"")),!b.parent("li").hasClass("active")){var e=c.find(".active:last a")[0],f=a.Event("show.bs.tab",{relatedTarget:e});if(b.trigger(f),!f.isDefaultPrevented()){var g=a(d);this.activate(b.parent("li"),c),this.activate(g,g.parent(),function(){b.trigger({type:"shown.bs.tab",relatedTarget:e})})}}},b.prototype.activate=function(b,c,d){function e(){f.removeClass("active").find("> .dropdown-menu > .active").removeClass("active"),b.addClass("active"),g?(b[0].offsetWidth,b.addClass("in")):b.removeClass("fade"),b.parent(".dropdown-menu")&&b.closest("li.dropdown").addClass("active"),d&&d()}var f=c.find("> .active"),g=d&&a.support.transition&&f.hasClass("fade");g?f.one(a.support.transition.end,e).emulateTransitionEnd(150):e(),f.removeClass("in")};var c=a.fn.tab;a.fn.tab=function(c){return this.each(function(){var d=a(this),e=d.data("bs.tab");e||d.data("bs.tab",e=new b(this)),"string"==typeof c&&e[c]()})},a.fn.tab.Constructor=b,a.fn.tab.noConflict=function(){return a.fn.tab=c,this},a(document).on("click.bs.tab.data-api",'[data-toggle="tab"], [data-toggle="pill"]',function(b){b.preventDefault(),a(this).tab("show")})}(jQuery),+function(a){"use strict";var b=function(c,d){this.options=a.extend({},b.DEFAULTS,d),this.$window=a(window).on("scroll.bs.affix.data-api",a.proxy(this.checkPosition,this)).on("click.bs.affix.data-api",a.proxy(this.checkPositionWithEventLoop,this)),this.$element=a(c),this.affixed=this.unpin=this.pinnedOffset=null,this.checkPosition()};b.RESET="affix affix-top affix-bottom",b.DEFAULTS={offset:0},b.prototype.getPinnedOffset=function(){if(this.pinnedOffset)return this.pinnedOffset;this.$element.removeClass(b.RESET).addClass("affix");var a=this.$window.scrollTop(),c=this.$element.offset();return this.pinnedOffset=c.top-a},b.prototype.checkPositionWithEventLoop=function(){setTimeout(a.proxy(this.checkPosition,this),1)},b.prototype.checkPosition=function(){if(this.$element.is(":visible")){var c=a(document).height(),d=this.$window.scrollTop(),e=this.$element.offset(),f=this.options.offset,g=f.top,h=f.bottom;"top"==this.affixed&&(e.top+=d),"object"!=typeof f&&(h=g=f),"function"==typeof g&&(g=f.top(this.$element)),"function"==typeof h&&(h=f.bottom(this.$element));var i=null!=this.unpin&&d+this.unpin<=e.top?!1:null!=h&&e.top+this.$element.height()>=c-h?"bottom":null!=g&&g>=d?"top":!1;if(this.affixed!==i){this.unpin&&this.$element.css("top","");var j="affix"+(i?"-"+i:""),k=a.Event(j+".bs.affix");this.$element.trigger(k),k.isDefaultPrevented()||(this.affixed=i,this.unpin="bottom"==i?this.getPinnedOffset():null,this.$element.removeClass(b.RESET).addClass(j).trigger(a.Event(j.replace("affix","affixed"))),"bottom"==i&&this.$element.offset({top:c-h-this.$element.height()}))}}};var c=a.fn.affix;a.fn.affix=function(c){return this.each(function(){var d=a(this),e=d.data("bs.affix"),f="object"==typeof c&&c;e||d.data("bs.affix",e=new b(this,f)),"string"==typeof c&&e[c]()})},a.fn.affix.Constructor=b,a.fn.affix.noConflict=function(){return a.fn.affix=c,this},a(window).on("load",function(){a('[data-spy="affix"]').each(function(){var b=a(this),c=b.data();c.offset=c.offset||{},c.offsetBottom&&(c.offset.bottom=c.offsetBottom),c.offsetTop&&(c.offset.top=c.offsetTop),b.affix(c)})})}(jQuery); -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/brought_by_horiz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/brought_by_horiz.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/common.css: -------------------------------------------------------------------------------- 1 | /* common.css - MoinMoin Default Styles 2 | 3 | Copyright (c) 2001, 2002, 2003 by Juergen Hermann 4 | */ 5 | 6 | /* content styles */ 7 | 8 | html { 9 | background-color: white; 10 | color: black; 11 | font-family: Helvetica, Verdana, Arial, Lucida Grande, sans-serif; 12 | font-size: 11pt; 13 | } 14 | 15 | body { 16 | margin: 0; 17 | } 18 | 19 | /* Links */ 20 | 21 | a {color: #2b7fcf;} 22 | a:visited {color: #2b7fcf;} 23 | a.nonexistent:visited, a.badinterwiki:visited, 24 | a.nonexistent, a.badinterwiki {color: gray;} 25 | 26 | a.www:before {content: url(../img/moin-www.png); margin: 0 0.2em;} 27 | a.http:before {content: url(../img/moin-www.png); margin: 0 0.2em;} 28 | a.https:before {content: url(../img/moin-www.png); margin: 0 0.2em;} 29 | a.file:before {content: url(../img/moin-ftp.png); margin: 0 0.2em;} 30 | a.ftp:before {content: url(../img/moin-ftp.png); margin: 0 0.2em;} 31 | a.nntp:before {content: url(../img/moin-news.png); margin: 0 0.2em;} 32 | a.news:before {content: url(../img/moin-news.png); margin: 0 0.2em;} 33 | a.telnet:before, a.ssh:before {content: url(../img/moin-telnet.png); margin: 0 0.2em;} 34 | a.irc:before, a.ircs:before {content: url(../img/moin-telnet.png); margin: 0 0.2em;} 35 | a.mailto:before {content: url(../img/moin-email.png); margin: 0 0.2em;} 36 | a.attachment:before {content: url(../img/moin-attach.png); margin: 0 0.2em;} 37 | a.badinterwiki:before {content: url(../img/moin-inter.png); margin: 0 0.2em;} 38 | a.interwiki:before {content: url(../img/moin-inter.png); margin: 0 0.2em;} 39 | 40 | 41 | li p { 42 | margin: .25em 0; 43 | } 44 | 45 | li.gap { 46 | margin-top: 0.5em; 47 | } 48 | 49 | dt { 50 | margin-top: 0.5em; 51 | font-weight: bold; 52 | } 53 | 54 | dd { 55 | margin-top: 0; 56 | margin-bottom: 0; 57 | } 58 | 59 | dd p { 60 | margin: 0.25em 0; 61 | } 62 | 63 | a, img, img.drawing { 64 | border: 0; 65 | } 66 | 67 | pre { 68 | border: 1pt solid #AEBDCC; 69 | background-color: #F3F5F7; 70 | padding: 5pt; 71 | font-family: courier, monospace; 72 | white-space: pre; 73 | /* begin css 3 or browser specific rules - do not remove! 74 | see: http://forums.techguy.org/archive/index.php/t-249849.html */ 75 | white-space: pre-wrap; 76 | word-wrap: break-word; 77 | white-space: -moz-pre-wrap; 78 | white-space: -pre-wrap; 79 | white-space: -o-pre-wrap; 80 | /* end css 3 or browser specific rules */ 81 | } 82 | 83 | pre.comment { 84 | background-color: #CCCCCC; 85 | color: red; 86 | padding: 0; 87 | margin: 0; 88 | border: 0; 89 | } 90 | 91 | pre.comment:before { 92 | content: url(../img/attention.png); 93 | } 94 | 95 | 96 | /* .comment css definition must be top of .red/.green/.blue or it won't work */ 97 | .comment { color: #555555; background-color: #DDDDFF; } 98 | 99 | .red { background-color: #FFCCCC; } 100 | .green { background-color: #CCFFCC; } 101 | .blue { background-color: #CCCCFF; } 102 | .yellow { background-color: #FFF29F; } 103 | .orange { background-color: #FFD59B; } 104 | 105 | .solid { border: 2px solid #000000; padding: 2px; } 106 | .dashed { border: 2px dashed #000000; padding: 2px; } 107 | .dotted { border: 2px dotted #000000; padding: 2px; } 108 | 109 | .left { text-align: left; } 110 | .center { text-align: center; } 111 | .right { text-align: right; } 112 | .justify { text-align: justify; } 113 | 114 | table 115 | { 116 | margin: 0.5em 0 0 0.5em; 117 | border-collapse: collapse; 118 | } 119 | 120 | th, td 121 | { 122 | padding: 0.25em 0.5em 0.25em 0.5em; 123 | /* border: 1pt solid #ADB9CC; */ 124 | } 125 | 126 | td p { 127 | margin: 0; 128 | padding: 0; 129 | } 130 | 131 | /* TableOfContents macro */ 132 | div.table-of-contents { 133 | border: 1px solid #bbbbbb; 134 | color: black; 135 | background-color: #eeeeee; 136 | font-size: 80%; 137 | text-align: left; 138 | margin: 0.5em 0 0.5em 1em; 139 | padding: 0.5em 0.75em 0.5em 0.5em; 140 | max-width: 50%; 141 | display: inline-table; 142 | } 143 | div.table-of-contents ol { 144 | margin: 0; 145 | padding: 0 0 0 2em; 146 | } 147 | div.table-of-contents ul { 148 | margin: 0; 149 | list-style:none; 150 | } 151 | div.table-of-contents li { 152 | margin:0; 153 | padding: 0; 154 | } 155 | p.table-of-contents-heading { 156 | font-weight:bold; 157 | padding:0; 158 | margin: 0 0 0.5em 0; 159 | letter-spacing: 0.075em; 160 | } 161 | 162 | /* Navigation macro */ 163 | table.navigation { 164 | background: #fff; 165 | margin: 0; 166 | } 167 | 168 | .footnotes div { 169 | width: 5em; 170 | border-top: 1pt solid gray; 171 | } 172 | 173 | .footnotes ol { 174 | padding: 0 2em; 175 | margin: 0 0 1em; 176 | } 177 | 178 | .info { 179 | float: right; 180 | font-size: 0.7em; 181 | color: gray; 182 | } 183 | 184 | #pageinfo { 185 | margin-top: 2em; 186 | } 187 | 188 | .seperator { 189 | color: gray; 190 | } 191 | 192 | #pagebottom {clear: both;} 193 | 194 | /* standard rule ---- */ 195 | hr { 196 | height: 1pt; 197 | background-color: #9C9C9C; 198 | border: 0; 199 | } 200 | 201 | /* custom rules ----- to ---------- */ 202 | .hr1 {height: 2pt;} 203 | .hr2 {height: 3pt;} 204 | .hr3 {height: 4pt;} 205 | .hr4 {height: 5pt;} 206 | .hr5 {height: 6pt;} 207 | .hr6 {height: 7pt;} 208 | 209 | /* Replacement for deprecated html 3 element and html 4 */ 210 | .u {text-decoration: underline;} 211 | .strike {text-decoration: line-through;} 212 | 213 | /* eye catchers */ 214 | .warning 215 | { 216 | color: red; 217 | } 218 | 219 | .error 220 | { 221 | color: red; 222 | } 223 | 224 | strong.highlight 225 | { 226 | background-color: #CCE0FF; 227 | padding: 1pt; 228 | } 229 | 230 | 231 | /* Recent changes */ 232 | 233 | .rcrss { 234 | float: right; 235 | margin: 0; 236 | } 237 | 238 | .recentchanges[dir="rtl"] .rcrss { 239 | float: left; 240 | } 241 | 242 | .recentchanges table { 243 | margin-left: 0; 244 | } 245 | 246 | .recentchanges td { 247 | vertical-align: top; 248 | border: none; 249 | border-bottom: 1pt solid #E6EAF0; 250 | background: #F2F4F7; 251 | } 252 | 253 | .rcdaybreak td { 254 | background: #B8C5D9; 255 | border: none; 256 | } 257 | 258 | .rcdaybreak td a { 259 | font-size: 0.88em; 260 | } 261 | 262 | .rcicon1, .rcicon2 { 263 | text-align: center; 264 | } 265 | 266 | .rcpagelink { 267 | width: 33%; 268 | } 269 | 270 | .rctime { 271 | font-size: 0.88em; 272 | white-space: nowrap; 273 | } 274 | 275 | .rceditor { 276 | white-space: nowrap; 277 | font-size: 0.88em; 278 | } 279 | 280 | .rccomment { 281 | width: 50%; 282 | color: gray; 283 | font-size: 0.88em; 284 | } 285 | 286 | 287 | /* User Preferences */ 288 | 289 | .userpref table, .userpref td { 290 | border: none; 291 | } 292 | 293 | /* CSS for new code_area markup used by Colorizer and ParserBase */ 294 | 295 | div.codearea { /* the div makes the border */ 296 | margin: 0.5em 0; 297 | padding: 0; 298 | border: 1pt solid #AEBDCC; 299 | background-color: #F3F5F7; 300 | color: black; 301 | } 302 | 303 | div.codearea pre { /* the pre has no border and is inside the div */ 304 | margin: 0; 305 | padding: 10pt; 306 | border: none; 307 | } 308 | 309 | a.codenumbers { /* format of the line numbering link */ 310 | margin: 0 10pt; 311 | font-size: 0.85em; 312 | color: gray; 313 | } 314 | 315 | /* format of certain syntax spans */ 316 | div.codearea pre span.LineNumber {color: gray;} 317 | div.codearea pre span.ID {color: #000000;} 318 | div.codearea pre span.Operator {color: #0000C0;} 319 | div.codearea pre span.Char {color: #004080;} 320 | div.codearea pre span.Comment {color: #008000;} 321 | div.codearea pre span.Number {color: #0080C0;} 322 | div.codearea pre span.String {color: #004080;} 323 | div.codearea pre span.SPChar {color: #0000C0;} 324 | div.codearea pre span.ResWord {color: #A00000;} 325 | div.codearea pre span.ConsWord {color: #008080; font-weight: bold;} 326 | div.codearea pre span.Error {color: #FF8080; border: solid 1.5pt #FF0000;} 327 | div.codearea pre span.ResWord2 {color: #0080ff; font-weight: bold;} 328 | div.codearea pre span.Special {color: #0000ff;} 329 | div.codearea pre span.Preprc {color: #803999;} 330 | 331 | /* for diff parser */ 332 | div.codearea pre span.DiffAdded {color: #4876FF;} 333 | div.codearea pre span.DiffRemoved {color: #FF0000;} 334 | div.codearea pre span.DiffChanged {color: #FF7F50;} 335 | div.codearea pre span.DiffSeparator {color: #228B22; font-weight: bold} 336 | 337 | /* Search results */ 338 | .advancedsearch { 339 | border: 1pt solid #ADB9CC; 340 | } 341 | 342 | .advancedsearch td { 343 | vertical-align: top; 344 | border: 0; 345 | } 346 | 347 | .advancedsearch td.searchfor { 348 | font-weight: bold; 349 | } 350 | 351 | .advancedsearch input { 352 | border: 1px solid #ADB9CC; 353 | } 354 | 355 | .advancedsearch td.submit { 356 | border-top: 1px solid #ADB9CC; 357 | text-align: right; 358 | } 359 | 360 | .advancedsearch optioni, select { 361 | border: 1px solid #ADB9CC; 362 | } 363 | 364 | 365 | .searchresults dt { 366 | margin-top: 1em; 367 | font-weight: normal; 368 | } 369 | 370 | .searchresults dd, .searchresults p { 371 | font-size: 0.85em; 372 | } 373 | 374 | .searchresults .searchhitinfobar { 375 | color: #008000; 376 | margin-left: 15px; 377 | margin-top: 0; 378 | } 379 | 380 | p.searchstats { 381 | font-size: 0.8em; 382 | text-align: right; 383 | width: 100%; 384 | background-color: #E6EAF0; 385 | border-top: 1px solid #9088DC; 386 | padding: 2px; 387 | } 388 | 389 | p.searchhint { 390 | background-color: #E6EAF0; 391 | border: 1px solid #9088DC; 392 | padding: 2px; 393 | } 394 | 395 | .searchpages { 396 | margin-left: auto; 397 | margin-right: auto; 398 | } 399 | 400 | .searchpages tr, .searchpages td { 401 | border: 0; 402 | padding: 5px; 403 | margin: 0; 404 | text-align: center; 405 | vertical-align: middle; 406 | color: #b93a58; 407 | font-weight: bold; 408 | font-size: 1.05em; 409 | } 410 | 411 | .searchpages td a, .searchpages td a:link { 412 | text-decoration: underline; 413 | } 414 | 415 | /* MonthCalendar css */ 416 | 417 | /* days without and with pages linked to them */ 418 | a.cal-emptyday { 419 | color: #777777; 420 | text-align: center; 421 | } 422 | a.cal-usedday { 423 | color: #000000; 424 | font-weight: bold; 425 | text-align: center; 426 | } 427 | /* general stuff: workdays, weekend, today */ 428 | td.cal-workday { 429 | background-color: #DDDDFF; 430 | text-align: center; 431 | } 432 | td.cal-weekend { 433 | background-color: #FFDDDD; 434 | text-align: center; 435 | } 436 | td.cal-today { 437 | background-color: #CCFFCC; 438 | border-style: solid; 439 | border-width: 2pt; 440 | text-align: center; 441 | } 442 | /* invalid places on the monthly calendar sheet */ 443 | td.cal-invalidday { 444 | background-color: #CCCCCC; 445 | } 446 | /* links to prev/next month/year */ 447 | a.cal-link { 448 | color: #000000; 449 | text-decoration: none; 450 | } 451 | th.cal-header { 452 | background-color: #DDBBFF; 453 | text-align: center; 454 | } 455 | 456 | /* for MonthCalendar mouseover info boxes */ 457 | table.tip { 458 | color: black; 459 | background-color: #FF8888; 460 | font-size: small; 461 | font-weight: normal; 462 | border-style: solid; 463 | border-width: 1px; 464 | } 465 | 466 | th.tip { 467 | background-color: #FF4444; 468 | font-weight: bold; 469 | text-align: center; 470 | } 471 | 472 | td.tip { 473 | text-align: left; 474 | } 475 | *[dir="rtl"] td.tip { 476 | text-align: right; 477 | } 478 | 479 | /* end MonthCalendar stuff */ 480 | 481 | #message .hint {font-style: italic;} 482 | #message .info { 483 | float: none; 484 | font-size: 1em; 485 | color: black; 486 | } 487 | #message .info:before { 488 | content: url('../img/icon-info.png'); 489 | margin: 0 0.5em 0 0.25em; 490 | vertical-align: middle; 491 | } 492 | #message .warning:before { 493 | content: url('../img/alert.png'); 494 | margin: 0 0.5em 0 0.125em; 495 | vertical-align: middle; 496 | } 497 | #message .error:before { 498 | content: url('../img/icon-error.png'); 499 | margin: 0 0.5em 0 0.125em; 500 | vertical-align: middle; 501 | } 502 | #message a.clear-link { 503 | display: block; 504 | margin: 0.75em 0 0 0; 505 | font-size: small; 506 | } 507 | 508 | 509 | /* admonition start */ 510 | #content div.caution, 511 | #content div.important, 512 | #content div.note, 513 | #content div.tip, 514 | #content div.warning { 515 | border: 1pt solid #E5E5E5; 516 | background-color: #F9F9FF; 517 | color: black; 518 | 519 | margin: 10pt 30pt 10pt 30pt; 520 | background-repeat: no-repeat; 521 | background-position: 8px 8px; 522 | min-height: 64px; /*64=48+8+8 but doesn't work with IE*/ 523 | padding-left: 64px; 524 | } 525 | 526 | #content div.caution p, 527 | #content div.important p, 528 | #content div.note p, 529 | #content div.tip p, 530 | #content div.warning p { 531 | margin-top: 8px; /*to align text with bg graphic*/ 532 | } 533 | 534 | #content div.tip { background-image: url("../img/admon-tip.png"); } 535 | #content div.note { background-image: url("../img/admon-note.png"); } 536 | #content div.important { background-image: url("../img/admon-important.png"); } 537 | #content div.caution { background-image: url("../img/admon-caution.png"); } 538 | #content div.warning { background-image: url("../img/admon-warning.png"); } 539 | 540 | /* section numbering */ 541 | 542 | #page { 543 | counter-reset: chapter; /* Create a chapter counter scope */ 544 | } 545 | #page h1 { 546 | counter-reset: chapter; 547 | } 548 | #page h2:before { 549 | content: counter(chapter) ". "; 550 | counter-increment: chapter; /* Add 1 to chapter */ 551 | } 552 | #page h2 { 553 | counter-reset: section; 554 | } 555 | #page h3 { 556 | counter-reset: subsection; 557 | } 558 | #page h3:before { 559 | content: counter(chapter) "." counter(section) " "; 560 | counter-increment: section; 561 | } 562 | #page h4:before { 563 | content: counter(chapter) "." counter(section) "." counter(subsection) " "; 564 | counter-increment: subsection; 565 | } 566 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/common.js: -------------------------------------------------------------------------------- 1 | // 2 | // MoinMoin commonly used JavaScript functions 3 | // 4 | 5 | // We keep here the state of the search box 6 | searchIsDisabled = false; 7 | 8 | function searchChange(e) { 9 | // Update search buttons status according to search box content. 10 | // Ignore empty or whitespace search term. 11 | var value = e.value.replace(/\s+/, ''); 12 | if (value == '' || searchIsDisabled) { 13 | searchSetDisabled(true); 14 | } else { 15 | searchSetDisabled(false); 16 | } 17 | } 18 | 19 | function searchSetDisabled(flag) { 20 | // Enable or disable search 21 | document.getElementById('fullsearch').disabled = flag; 22 | document.getElementById('titlesearch').disabled = flag; 23 | } 24 | 25 | function searchFocus(e) { 26 | // Update search input content on focus 27 | if (e.value == search_hint) { 28 | e.value = ''; 29 | e.className = ''; 30 | searchIsDisabled = false; 31 | } 32 | } 33 | 34 | function searchBlur(e) { 35 | // Update search input content on blur 36 | if (e.value == '') { 37 | e.value = search_hint; 38 | e.className = 'disabled'; 39 | searchIsDisabled = true; 40 | } 41 | } 42 | 43 | function actionsMenuInit(title) { 44 | // Initialize action menu 45 | for (i = 0; i < document.forms.length; i++) { 46 | var form = document.forms[i]; 47 | if (form.className == 'actionsmenu') { 48 | // Check if this form needs update 49 | var div = form.getElementsByTagName('div')[0]; 50 | var label = div.getElementsByTagName('label')[0]; 51 | if (label) { 52 | // This is the first time: remove label and do buton. 53 | div.removeChild(label); 54 | var dobutton = div.getElementsByTagName('input')[0]; 55 | div.removeChild(dobutton); 56 | // and add menu title 57 | var select = div.getElementsByTagName('select')[0]; 58 | var item = document.createElement('option'); 59 | item.appendChild(document.createTextNode(title)); 60 | item.value = 'show'; 61 | select.insertBefore(item, select.options[0]); 62 | select.selectedIndex = 0; 63 | } 64 | } 65 | } 66 | } 67 | 68 | // use this instead of assigning to window.onload directly: 69 | function addLoadEvent(func) { 70 | // alert("addLoadEvent " + func) 71 | var oldonload = window.onload; 72 | if (typeof window.onload != 'function') { 73 | window.onload = func; 74 | } else { 75 | window.onload = function() { 76 | oldonload(); 77 | func(); 78 | } 79 | } 80 | } 81 | 82 | // copy from fckeditor browser check code (fckeditor.js:298, function : FCKeditor_IsCompatibleBrowser) 83 | function can_use_gui_editor() { 84 | var sAgent = navigator.userAgent.toLowerCase() ; 85 | 86 | // Internet Explorer 5.5+ 87 | if ( /*@cc_on!@*/false && sAgent.indexOf("mac") == -1 ) 88 | { 89 | var sBrowserVersion = navigator.appVersion.match(/MSIE (.\..)/)[1] ; 90 | return ( sBrowserVersion >= 5.5 ) ; 91 | } 92 | 93 | // Gecko (Opera 9 tries to behave like Gecko at this point). 94 | if ( navigator.product == "Gecko" && navigator.productSub >= 20030210 && !( typeof(opera) == 'object' && opera.postError ) ) 95 | return true ; 96 | 97 | // Opera 9.50+ 98 | if ( window.opera && window.opera.version && parseFloat( window.opera.version() ) >= 9.5 ) 99 | return true ; 100 | 101 | // Adobe AIR 102 | // Checked before Safari because AIR have the WebKit rich text editor 103 | // features from Safari 3.0.4, but the version reported is 420. 104 | if ( sAgent.indexOf( ' adobeair/' ) != -1 ) 105 | return ( sAgent.match( / adobeair\/(\d+)/ )[1] >= 1 ) ; // Build must be at least v1 106 | 107 | // Safari 3+ 108 | if ( sAgent.indexOf( ' applewebkit/' ) != -1 ) 109 | return ( sAgent.match( / applewebkit\/(\d+)/ )[1] >= 522 ) ; // Build must be at least 522 (v3) 110 | 111 | return false ; 112 | 113 | } 114 | 115 | 116 | function update_edit_links() { 117 | // Update editlink according if if the browser is compatible 118 | if (can_use_gui_editor() == false){ 119 | //alert("update_edit_links: can't use gui editor"); 120 | return; 121 | } 122 | var editlinks = document.getElementsByName("editlink"); 123 | for (i = 0; i < editlinks.length; i++) { 124 | var link = editlinks[i]; 125 | href = link.href.replace('editor=textonly','editor=guipossible'); 126 | link.href = href; 127 | //alert("update_edit_links: modified to guipossible"); 128 | } 129 | } 130 | 131 | 132 | function add_gui_editor_links() { 133 | // Add gui editor link after the text editor link 134 | 135 | // If the variable is not set or browser is not compatible, exit 136 | try {gui_editor_link_href} 137 | catch (e) { 138 | //alert("add_gui_editor_links: gui_editor_link_href not here"); 139 | return 140 | } 141 | if (can_use_gui_editor() == false){ 142 | //alert("add_gui_editor_links: can't use gui_editor"); 143 | return; 144 | } 145 | var all = document.getElementsByName('texteditlink'); 146 | for (i = 0; i < all.length; i++) { 147 | var textEditorLink = all[i]; 148 | // Create a list item with a link 149 | var guiEditorLink = document.createElement('a'); 150 | guiEditorLink.href = gui_editor_link_href; 151 | var text = document.createTextNode(gui_editor_link_text); 152 | guiEditorLink.appendChild(text); 153 | var listItem = document.createElement('li') 154 | listItem.appendChild(guiEditorLink); 155 | // Insert in the editbar 156 | var editbar = textEditorLink.parentNode.parentNode 157 | var nextListItem = textEditorLink.parentNode.nextSibling; 158 | editbar.insertBefore(listItem, nextListItem); 159 | //alert("add_gui_editor_links: added gui editor link"); 160 | } 161 | } 162 | 163 | 164 | function show_switch2gui() { 165 | // Show switch to gui editor link if the browser is compatible 166 | if (can_use_gui_editor() == false) return; 167 | 168 | var switch2gui = document.getElementById('switch2gui') 169 | if (switch2gui) { 170 | switch2gui.style.display = 'inline'; 171 | } 172 | } 173 | 174 | // for long documents with many comments this is expensive to calculate, 175 | // thus we keep it here: 176 | comments = null; 177 | 178 | function toggleComments() { 179 | // Toggle visibility of every tag with class "comment" 180 | for (i = 0; i < comments.length; i++){ 181 | el = comments[i]; 182 | if ( el.style.display != 'none' ) { 183 | el.style.display = 'none'; 184 | } else { 185 | el.style.display = ''; 186 | } 187 | } 188 | } 189 | 190 | function show_toggleComments() { 191 | // Show edit bar item for toggling inline comments on/off only if inline comments exist on the page 192 | comments = getElementsByClassName('comment', null, document); 193 | if (comments.length > 0) { 194 | var buttons = getElementsByClassName('toggleCommentsButton', null, document); 195 | for (i = 0; i < buttons.length; i++){ 196 | el = buttons[i]; 197 | el.style.display = ''; 198 | } 199 | } 200 | } 201 | 202 | 203 | function load() { 204 | // Do not name this "onload", it does not work with IE :-) 205 | // TODO: create separate onload for each type of view and set the 206 | // correct function name in the html. 207 | // e.g 208 | 209 | // login focus 210 | if (document.forms['loginform']) { 211 | document.forms['loginform'].elements['name'].focus(); 212 | } 213 | 214 | // Page view stuff 215 | update_edit_links(); 216 | add_gui_editor_links(); 217 | 218 | // Editor stuff 219 | show_switch2gui(); 220 | 221 | // Enable menu item "ToggleComments" if inline comments exist 222 | show_toggleComments(); 223 | 224 | // data browser widget 225 | dbw_hide_buttons(); 226 | } 227 | 228 | 229 | function before_unload(evt) { 230 | // TODO: Better to set this in the editor html, as it does not make 231 | // sense elsehwere. 232 | // confirmleaving is available when editing 233 | try {return confirmleaving();} 234 | catch (e) {} 235 | } 236 | 237 | // Initialize after loading the page 238 | addLoadEvent(load) 239 | 240 | // Catch before unloading the page 241 | window.onbeforeunload = before_unload 242 | 243 | function dbw_update_search(dbw_id) 244 | { 245 | var table = document.getElementById(dbw_id+'table'); 246 | var cell; 247 | var shown; 248 | var i 249 | var cols = table.rows[0].cells.length; 250 | var filter = new Array(); 251 | var dofilter = new Array(); 252 | var form = document.forms[dbw_id+'form']; 253 | 254 | for (i = 0; i < cols; i++) { 255 | dofilter[i] = false; 256 | if (form[dbw_id+'filter'+i]) { 257 | dofilter[i] = true; 258 | filter[i] = form[dbw_id+'filter'+i].value; 259 | if (filter[i] == '[all]') 260 | dofilter[i] = false; 261 | if (filter[i] == '[empty]') 262 | filter[i] = ''; 263 | } 264 | } 265 | 266 | for (i = 1; i < table.rows.length; i++) { 267 | var show = true; 268 | for (col = 0; col < cols; col++) { 269 | if (!dofilter[col]) 270 | continue; 271 | 272 | cell = table.rows[i].cells[col]; 273 | 274 | if (filter[col] == '[notempty]') { 275 | if (cell.abbr == '') { 276 | show = false; 277 | break; 278 | } 279 | } else if (filter[col] != cell.abbr) { 280 | show = false; 281 | break; 282 | } 283 | } 284 | if (show) 285 | table.rows[i].style.display = ''; 286 | else 287 | table.rows[i].style.display = 'none'; 288 | } 289 | } 290 | 291 | function dbw_hide_buttons() { 292 | var form; 293 | var elem; 294 | var name; 295 | 296 | for (var fidx = 0; fidx < document.forms.length; fidx++) { 297 | form = document.forms[fidx]; 298 | for (var eidx = 0; eidx < form.elements.length; eidx++) { 299 | elem = form.elements[eidx]; 300 | name = elem.name; 301 | if (name) { 302 | if (name.indexOf('dbw.') >= 0 && name.substr(-7) == '.submit') 303 | elem.style.display = 'none'; 304 | } 305 | } 306 | } 307 | } 308 | 309 | /* getElementsByClassName 310 | Developed by Robert Nyman, http://www.robertnyman.com 311 | Code/licensing: http://code.google.com/p/getelementsbyclassname/ (MIT license) 312 | Version: 1.0.1 313 | */ 314 | var getElementsByClassName = function (className, tag, elm){ 315 | if (document.getElementsByClassName) { 316 | getElementsByClassName = function (className, tag, elm) { 317 | elm = elm || document; 318 | var elements = elm.getElementsByClassName(className), 319 | nodeName = (tag)? new RegExp("\\b" + tag + "\\b", "i") : null, 320 | returnElements = [], 321 | current; 322 | for(var i=0, il=elements.length; i> and ---- (horizontal rule) 613 | //~ // But this effects paragraphs on multiple lines: doubleclick goes to paragraph bottom rather than top. 614 | //~ // Seems best to live with TOC and HR problem and wait for Moin2. 615 | //~ var next1, next2, next3; 616 | //~ var nextNbr = 'line-' + (lineNbr-0+1); 617 | //~ if (someNode.parentNode.tagName == 'P' && someNode.parentNode.scrollLine) { 618 | //~ next1 = someNode.nextSibling; 619 | //~ if (next1 && next1.tagName != 'SPAN') { 620 | //~ next2 = next1.nextSibling; 621 | //~ } 622 | //~ if (next2 && next2.id == nextNbr) { 623 | //~ alert('Correcting scrollLine='+lineNbr); 624 | //~ someNode.parentNode.scrollLine = lineNbr; 625 | //~ return; 626 | //~ } 627 | //~ } 628 | 629 | var ie8LoopCounter = 0; 630 | var doChild = true; 631 | while (!(someNode.id == nextId) && !(someNode.id == topId)) { 632 | // workaround IE8 bug: http://moinmo.in/MoinMoinBugs/FormInsideTableCausesIE8ScriptLoop 633 | ie8LoopCounter += 1; 634 | if (ie8LoopCounter > 10000) { 635 | return; 636 | } 637 | 638 | // add children, add siblings, add parent 639 | if (doChild && someNode.firstChild) { 640 | someNode = someNode.firstChild; 641 | } else { 642 | doChild = true; 643 | if (someNode.nextSibling) { 644 | someNode = someNode.nextSibling; 645 | } else { 646 | if (someNode.parentNode.nextSibling) { 647 | someNode = someNode.parentNode.nextSibling; 648 | } else { 649 | doChild = false; 650 | someNode = someNode.parentNode.parentNode; 651 | } 652 | } 653 | } 654 | if (doChild && someNode.tagName && !(someNode.id == nextId) && !(someNode.id == topId)) { 655 | setCallback(someNode, lineNbr, isPreview); 656 | } 657 | } 658 | } 659 | 660 | // run during page load when user may edit current page OR is viewing draft preview 661 | function setSpanTags(isPreview) { 662 | var startTime = new Date(); 663 | // find all the SPAN tags with an ID beginning with "line-" 664 | var spanTags = document.getElementsByTagName('span'); 665 | var marks = []; 666 | for (var i = 0; i < spanTags.length; ++i) { 667 | if (spanTags[i].id && spanTags[i].id.substring(0, 5) == 'line-') { 668 | marks.push(spanTags[i]); 669 | } 670 | } 671 | var top = document.getElementById('content'); 672 | var bottom = document.getElementById('bottom'); 673 | // add expected stopping point to end of array for convenience 674 | if (bottom) { 675 | marks.push(bottom); 676 | } else { 677 | if (autoScrollDebugOn) { 678 | alert("auto scroll debug 1: document.getElementById('bottom') failed"); 679 | } 680 | } 681 | var skipTo = -1; 682 | // loop through span tags and apply double-click events to appropriate node(s) 683 | for (i = 0; i < marks.length-1; ++i) { 684 | var mark = marks[i]; 685 | // skip span tags generated by embedded parsers 686 | if (i > skipTo) { 687 | // split the ID into parts: looks like "line-22" or "line-22-1" 688 | var lineParts = mark.id.split('-'); 689 | var line = lineParts[1]; 690 | if (lineParts.length == 3) { 691 | // have found output from embedded parser 692 | // find next span id that looks like "line-n" and the "line-n-n" just before it 693 | var j = i - 0; 694 | while (lineParts.length == 3) { 695 | j++; 696 | lineParts = marks[j].id.split('-'); 697 | } 698 | // determine how many lines, starting line number, and add double-click events 699 | var nbrParsedLines = j - i; 700 | var parsedLineNbr = lineParts[1] - nbrParsedLines - 1; 701 | for (var k = 0; k < nbrParsedLines; ++k) { 702 | if (marks[i+k] && marks[i+k+1] && marks[i+k+1].id) { 703 | walkDom (marks[i+k], parsedLineNbr+k, isPreview, marks[i+k+1].id, top.id); 704 | } else { 705 | if (autoScrollDebugOn) { 706 | alert('auto scroll debug 2: skipping walkDom, i=' + i + ' k=' + k + ' marks[i].id=' + marks[i].id); 707 | } 708 | } 709 | } 710 | // done with embedded parser lines, tell main loop to skip these 711 | skipTo = j - 1; 712 | } else { 713 | // walk part of DOM and apply doubleclick function to every node with a tagname 714 | if (marks[i+1] && marks[i+1].id) { 715 | walkDom (mark, line, isPreview, marks[i+1].id, top.id); 716 | } else { 717 | if (autoScrollDebugOn) { 718 | alert('auto scroll debug 3: skipping walkDom, i=' + i + ' marks[i].id=' + marks[i].id); 719 | } 720 | } 721 | } 722 | } 723 | } 724 | if (autoScrollDebugOn && document.getElementById('content')) { 725 | for (i = 0; i < marks.length-1; ++i) { 726 | marks[i].innerHTML = ' ' + marks[i].id + ' '; 727 | marks[i].style. color = "red"; 728 | } 729 | showStartStopTimes(startTime); 730 | } 731 | } 732 | 733 | // test to see if this user has selected or defaulted to edit_on_doubleclick AND 734 | // whether we are viewing a page, editing a page, or previewing an edit draft 735 | function scrollTextareaInit() { 736 | // look for meta tag -- is edit_on_doubleclick present? 737 | if (!document.getElementsByName('edit_on_doubleclick').length) { 738 | return; 739 | } 740 | turnDebugOnOrOff (); 741 | // are we viewing a page - both gui and text editors will have button named button_save 742 | if (!document.getElementsByName('button_save').length) { 743 | setSpanTags(0); 744 | return; 745 | } 746 | // we are in editor -- is there a line number specified in URL? 747 | var lineMatch = document.location.search.match(/line=(\d*)/); 748 | if (lineMatch) { 749 | scrollTextarea(lineMatch[1]); 750 | return; 751 | } 752 | if (document.getElementById('preview')) { 753 | // is an editor preview 754 | setSpanTags(1); 755 | } 756 | } 757 | 758 | // The DOM ready check for Internet Explorer 759 | function ieScrollCheck() { 760 | try { 761 | // If IE is used, use the trick by Diego Perini 762 | document.documentElement.doScroll("left"); 763 | } catch( error ) { 764 | setTimeout( ieScrollCheck, 1 ); 765 | return; 766 | } 767 | scrollTextareaInit(); 768 | } 769 | 770 | // run auto scroll init As Soon As Possible -- prior to onload for modern browsers 771 | function runASAP() { 772 | if (document.addEventListener) { 773 | // Firefox 3.6, Chrome 4.0.249.89, Safari for Windows 4.04, Opera 10.5beta, and maybe older versions 774 | // schedule func to be run when DOM complete, usually before last image loaded 775 | document.addEventListener("DOMContentLoaded", scrollTextareaInit, false); 776 | } else { 777 | if (document.documentElement.doScroll && window == window.top) { 778 | // IE 5-8 and not using frames 779 | ieScrollCheck(); 780 | } else { 781 | addLoadEvent(scrollTextareaInit); 782 | } 783 | } 784 | } 785 | // auto scroll initialization starts here 786 | runASAP(); 787 | 788 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/loam_velodyne.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/loam_velodyne.jpeg -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/loam_velodyne_002.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/loam_velodyne_002.jpeg -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/loam_velodyne_003.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/loam_velodyne_003.jpeg -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/loam_velodyne_004.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/loam_velodyne_004.jpeg -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_browse_software.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_browse_software.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_documentation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_documentation.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_download.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_download.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_left.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_news.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_news.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_right.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_spacer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/menu_spacer.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/print.css: -------------------------------------------------------------------------------- 1 | /* print.css - MoinMoin Default Styles 2 | 3 | Copyright (c) 2001, 2002, 2003 by Juergen Hermann 4 | */ 5 | 6 | /* content styles */ 7 | 8 | html { 9 | font-family: Times, serif; 10 | font-size: 12pt; 11 | } 12 | 13 | body { 14 | /* Give about 3.4cm in Mozilla/Firefox and about 2.2cm in Safari */ 15 | margin: 1.5cm; 16 | } 17 | 18 | a, a:visited, a.nonexistent, a.badinterwiki { 19 | color: black; 20 | text-decoration: none; 21 | } 22 | 23 | a:hover { 24 | text-decoration: underline; 25 | } 26 | 27 | .info a { 28 | color: gray; 29 | } 30 | 31 | pre { 32 | font-size: 10pt; 33 | } 34 | 35 | a.interwiki:before, a.badinterwiki:before { 36 | content: attr(title) ":"; 37 | } 38 | 39 | a.interwiki img, a.badinterwiki img { 40 | display: none; 41 | } 42 | 43 | .footnotes div { 44 | width: 5em; 45 | border-top: 1pt solid gray; 46 | } 47 | 48 | /* user interface styles */ 49 | 50 | #header, #sidebar, #footer, #timings, #credits, #interwiki, #pagelocation { 51 | display: none; 52 | } 53 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/projection.css: -------------------------------------------------------------------------------- 1 | /* projection.css - MoinMoin Slide Styles 2 | 3 | Copyright (c) 2003 by Juergen Hermann 4 | */ 5 | 6 | @import url(screen.css); 7 | 8 | html { line-height: 1.8em; } 9 | 10 | body, b, em, a, span, div, p, td { font-size: 22pt; } 11 | 12 | h1 { font-size: 28pt; } 13 | h2 { font-size: 24pt; } 14 | h3 { font-size: 22pt; } 15 | h4 { font-size: 20pt; } 16 | h5 { font-size: 18pt; } 17 | h6 { font-size: 16pt; } 18 | 19 | tt,pre { font-size: 18pt; } 20 | sup, sub { font-size: 14pt; } 21 | 22 | .navigation { 23 | font-size: 16pt; 24 | padding-top: 8pt; 25 | padding-bottom: 8pt; 26 | } 27 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/ros_org.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/ros_org.png -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/rosversion.js: -------------------------------------------------------------------------------- 1 | // version.js, based on SeeSaw 1.0 2 | function Version(sections) { 3 | var dotversion = ".version."; 4 | 5 | // Tag shows unless already tagged 6 | $.each(sections.show, function(index, value) { 7 | $("span" + dotversion + value).not(".versionshow,.versionhide") 8 | .filter(".hidepart") 9 | .addClass("versionshow") 10 | .end() 11 | .filter(".showpart") 12 | .addClass("versionhide"); 13 | }); 14 | $.each(sections.show, function(index, value) { 15 | $("div" + dotversion + value).not(".versionshow,.versionhide") 16 | .addClass("versionshow"); 17 | }); 18 | 19 | // Tag hides unless already tagged 20 | $.each(sections.hide, function(index, value) { 21 | $("span" + dotversion + value).not(".versionshow,.versionhide") 22 | .filter(".showpart") 23 | .addClass("versionshow") 24 | .end() 25 | .filter(".hidepart") 26 | .addClass("versionhide"); 27 | }); 28 | $.each(sections.hide, function(index, value) { 29 | $("div" + dotversion + value).not(".versionshow,.versionhide") 30 | .addClass("versionhide"); 31 | }); 32 | 33 | // Show or hide according to tag 34 | $(".versionshow").removeClass("versionshow").filter("span").show().end().filter("div").show(); 35 | $(".versionhide").removeClass("versionhide").filter("span").hide().end().filter("div").hide(); 36 | 37 | $(".rosversion_name").text(sections.target_ros_distro); 38 | } 39 | 40 | function getURLParameter(name) { 41 | return decodeURIComponent( 42 | ( 43 | new RegExp('[?|&]' + name + '=' + '([^&;]+?)(&|#|;|$)').exec(location.search) || [,""] 44 | )[1].replace(/\+/g, '%20')) || null; 45 | } 46 | 47 | function toggleDocStatus() 48 | { 49 | if ($("#doc_status").is(":hidden")) { 50 | $("#doc_status").slideDown(); 51 | } else { 52 | $("#doc_status").slideUp(); 53 | } 54 | } 55 | 56 | $(document).ready(function() { 57 | var activedistro = "jade"; //CHANGE THIS LINE TO CHANGE THE DISTRO DISPLAYED BY DEFAULT 58 | var url_distro = getURLParameter('distro'); 59 | if (url_distro) { 60 | activedistro=url_distro; 61 | } 62 | // Make the $ROS_DISTRO replacement work by wrapping it in a span. This is 63 | // necessary vs. MoinMoin macros because macros are not expanded inside of 64 | // code blocks, where this replacement is most useful. Using a function for 65 | // the replacement allows supporting escapes, so that the following transformations 66 | // are done: 67 | // $ROS_DISTRO -> hydro, indigo, etc. 68 | // \$ROS_DISTRO -> $ROS_DISTRO 69 | // \\$ROS_DISTRO -> \$ROS_DISTRO 70 | $("#page p:contains($ROS_DISTRO), #page pre:contains($ROS_DISTRO)").each(function() { 71 | $(this).html($(this).html().replace(/\\?\$ROS_DISTRO/g, 72 | function(match) { 73 | if (match[0] == "\\") { 74 | return "$ROS_DISTRO"; 75 | } else { 76 | return "$ROS_DISTRO"; 77 | } 78 | }) 79 | ); 80 | }); 81 | $("div.version").hide(); 82 | if ($("#"+activedistro).length > 0) { 83 | $("#"+activedistro).click(); 84 | } else { 85 | $("#rosversion_selector button:last").click(); 86 | } 87 | $("input.version:hidden").each(function() { 88 | var bg = $(this).attr("value").split(":"); 89 | $("div.version." + bg[0]).css("background-color",bg[1]).removeClass(bg[0]); 90 | }); 91 | }); 92 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/roswiki.js: -------------------------------------------------------------------------------- 1 | function toggleExpandable(id) { 2 | var d = document.getElementById(id); 3 | d.style.display = (d.style.display != 'none' ? 'none' : '' ); 4 | } 5 | 6 | function toggleExpandableJenkins(id) { 7 | toggleExpandable(id); 8 | var build_status_image = $('#' + id).attr('build_status_image'); 9 | if (!build_status_image) { 10 | $('#' + id).attr('build_status_image', true); 11 | $('#' + id + ' a').each(function() { 12 | var href = this.href; 13 | var n = href.lastIndexOf('/', href.length - 2); 14 | if (n != -1) { 15 | var jobname = href.substring(n +1); 16 | $('
    ').insertAfter(this); 17 | } 18 | }); 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/screen.css: -------------------------------------------------------------------------------- 1 | /* screen.css - MoinMoin Default Styles 2 | 3 | Copyright (c) 2001, 2002, 2003 by Juergen Hermann 4 | */ 5 | 6 | /* content styles */ 7 | 8 | body { 9 | margin: 0; 10 | padding: 0; 11 | border: 0; 12 | } 13 | 14 | #pageinfo { 15 | font-size: 10pt; 16 | } 17 | a.download { 18 | font-size: 120%; 19 | letter-spacing: 0.05em; 20 | font-weight: bold; 21 | background: #E7E7E7; 22 | border: 1px solid #9C9C9C; 23 | padding: 0.5em; 24 | text-align: center; 25 | } 26 | 27 | .footnotes div { 28 | width: 5em; 29 | border-top: 1px solid #e5e5e5; 30 | } 31 | 32 | input { 33 | font-size: 1em; 34 | font-family: Arial, Lucida Grande, sans-serif; 35 | } 36 | 37 | textarea { 38 | font-size: 1em; 39 | font-family: monospace; 40 | } 41 | 42 | span.sep { 43 | display: none; 44 | } 45 | 46 | /* user interface styles */ 47 | 48 | #logo { 49 | float: left; 50 | margin: 5px 10px; 51 | padding: 0; 52 | /* For text only logo */ 53 | font-size: 1.4em; 54 | line-height: 1em; 55 | font-weight: bold; 56 | } 57 | 58 | *[dir="rtl"] #logo { 59 | float: right; 60 | } 61 | 62 | #logo img { 63 | vertical-align: middle; 64 | } 65 | 66 | #logo a { 67 | color: black; 68 | text-decoration: none; 69 | } 70 | 71 | #searchform { 72 | float: right; 73 | margin: 5px 10px 0 10px; 74 | padding: 0; 75 | white-space: nowrap; 76 | } 77 | 78 | *[dir="rtl"] #searchform { 79 | float: left; 80 | } 81 | 82 | #searchform form div { 83 | display: inline; 84 | } 85 | 86 | #dpage { 87 | width: 1024px; 88 | margin-left: auto; 89 | margin-right: auto; 90 | } 91 | #page { 92 | /* margin: 10px 220px 10px 10px; */ 93 | 94 | padding: 4px; 95 | margin-left: 10px; 96 | width: 780px; 97 | } 98 | 99 | .footer { 100 | margin-left: 10px; 101 | clear: all; 102 | font-size: 10pt; 103 | color: #999; 104 | } 105 | #sidebar { 106 | display: inline; 107 | margin: 10px; 108 | float: right; 109 | clear: right; 110 | width: 200px; 111 | padding: 0; 112 | font-size: 0.88em; 113 | overflow: hidden; 114 | } 115 | 116 | .sidepanel { 117 | margin: 0 0 10px 0; 118 | float: right; 119 | width: 180px; 120 | } 121 | 122 | .sidepanel h1 { 123 | margin: 0; 124 | padding: 0.2em 10px; 125 | text-align: center; 126 | border: none; 127 | border-top: 1px solid white; 128 | border-bottom: 1pt solid #BFAE8F; 129 | font-size: 1em; 130 | color: black; 131 | } 132 | 133 | .sidepanel h2 { 134 | margin: 0; 135 | padding: 0.2em 10px; 136 | font-size: 1em; 137 | border: none; 138 | color: black; 139 | /*background: #BFAE8F; */ 140 | } 141 | 142 | .sidepanel ul { 143 | list-style-type: none; 144 | margin: 0; 145 | padding: 0; 146 | } 147 | 148 | .sidepanel li { 149 | margin: 0; 150 | padding: 2px 10px; 151 | } 152 | 153 | .sidepanel li form { 154 | margin: 0; 155 | } 156 | 157 | .sidepanel a { 158 | text-decoration: none; 159 | display: block; 160 | } 161 | 162 | .sidepanel a:hover { 163 | text-decoration: underline; 164 | } 165 | 166 | .actionsmenu select { 167 | width: 160px; 168 | } 169 | 170 | #pagetrail { 171 | clear: right; 172 | margin: 5px 11px; 173 | padding: 0; 174 | font-size: 0.88em; 175 | } 176 | 177 | *[dir="rtl"] #pagetrail { 178 | clear: left; 179 | margin: 0 11px; 180 | } 181 | 182 | #interwiki { 183 | margin: 5px 11px; 184 | display: inline; 185 | font-size: 1em; 186 | } 187 | 188 | *[dir="rtl"] #interwiki { 189 | margin: 0; 190 | } 191 | 192 | #interwiki span:after { 193 | content: ":"; 194 | } 195 | 196 | #locationline { 197 | clear: right; 198 | padding: 0; 199 | } 200 | 201 | *[dir="rtl"] #locationline { 202 | clear: left; 203 | } 204 | 205 | #header2 { 206 | margin-top: 20px; 207 | margin-bottom: 0; 208 | } 209 | #pagelocation { 210 | display: inline; 211 | margin: 0 0 0 10px; 212 | padding: 0; 213 | font-size: 18pt; 214 | font-weight: bold; 215 | } 216 | #pagelocation a { 217 | text-decoration: none; 218 | } 219 | 220 | #pagetrail li, #pagelocation li { 221 | display: inline; 222 | margin: 0; 223 | } 224 | 225 | /* XXX Warning: non-ascii characters! */ 226 | #pagetrail li:after { 227 | content: " » "; 228 | } 229 | 230 | *[dir="rtl"] #pagetrail li:after { 231 | content: " « "; 232 | } 233 | 234 | #pagetrail li:last-child:after { 235 | content: ""; 236 | } 237 | 238 | #pagelocation li:after { 239 | content: "/ "; 240 | } 241 | 242 | *[dir="rtl"] #pagelocation li:after { 243 | content: " \\ "; /* TODO: check what looks best with RTL */ 244 | } 245 | 246 | #pagelocation li:last-child:after { 247 | content: ""; 248 | } 249 | 250 | #navibar li.current { 251 | border: none; 252 | background: #eee; 253 | } 254 | 255 | .editbar form, .editbar form div { 256 | display: inline; 257 | margin: 0; 258 | } 259 | 260 | #message { 261 | margin: 10px 220px 10px 10px; 262 | padding: 4px; 263 | background-color: #F0ECE6; 264 | border: 1px solid #9c9c9c; 265 | border-bottom: 2px solid #9c9c9c; 266 | } 267 | 268 | #message p { 269 | margin: 5px 0; 270 | padding: 0; 271 | font-weight: bold; 272 | } 273 | 274 | #message div.buttons { 275 | font-weight: normal; 276 | } 277 | 278 | form.dialog { 279 | margin: 0 15px; 280 | } 281 | 282 | .dialog td { 283 | border: none; 284 | padding: 5px; 285 | } 286 | 287 | .dialog td.label { 288 | text-align: right; 289 | font-weight: bold; 290 | width: 25%; 291 | } 292 | 293 | *[dir="rtl"] .dialog td.label { 294 | text-align: left; 295 | } 296 | 297 | .dialog td.content input { 298 | width: 100%; 299 | } 300 | 301 | /* We use here dumb css1 ids because of IE suckiness */ 302 | #editor-comment {width: 70%;} 303 | #editor-textarea {width: 100%;} 304 | 305 | #preview, #previewbelow { 306 | border: 2px solid #e5e5e5; 307 | padding: .5em; 308 | background: url(../img/draft.png); 309 | } 310 | 311 | #textcha { 312 | font-size: 100%; 313 | margin-top: 0.5em; 314 | border: 2px solid #FF8888; 315 | color: black; 316 | vertical-align: middle; 317 | padding: 3px 2px; 318 | } 319 | 320 | #textcha-answer { 321 | border: 2px solid #000000; 322 | padding: 3px 2px; 323 | } 324 | 325 | #pagebottom { 326 | clear: both; 327 | } 328 | 329 | #footer { 330 | clear: both; 331 | margin: 0; 332 | padding: 0; 333 | } 334 | 335 | #credits, #version, #timings{ 336 | margin: 5px 10px; 337 | padding: 0; 338 | text-align: center; 339 | font-size: 0.88em; 340 | color: #6C7680; 341 | } 342 | 343 | #credits li, #timings li { 344 | display: inline; 345 | padding: 0 2px; 346 | margin: 0 4px; 347 | } 348 | 349 | #credits img { 350 | vertical-align: middle; 351 | } 352 | 353 | .diff { 354 | width: 99%; 355 | } 356 | 357 | .diff-title { 358 | background-color: #C0C0C0; 359 | } 360 | 361 | .diff-added { 362 | background-color: #E0FFE0; 363 | vertical-align: sub; 364 | } 365 | 366 | .diff-removed { 367 | background-color: #FFFFE0; 368 | vertical-align: sub; 369 | } 370 | 371 | .diff-added span { 372 | background-color: #80FF80; 373 | } 374 | 375 | .diff-removed span { 376 | background-color: #FFFF80; 377 | } 378 | 379 | .searchresult dd span { 380 | font-weight: bold; 381 | } 382 | 383 | #openididentifier { 384 | background: url(../../common/openid.png) no-repeat; 385 | background-position: 0 50%; 386 | padding-left: 18px; 387 | } 388 | 389 | #topnav-table { 390 | border: 0; 391 | margin-left: auto; 392 | margin-right: auto; 393 | } 394 | .package-links { 395 | /* float this between the content and the Wiki sidebar. there is the issue that large package names can bleed into the right sidebar if they exceed the width */ 396 | float: right; 397 | width: 200px; 398 | padding: 5px; 399 | margin-right: 0; 400 | margin-left: 5px; 401 | background-color: #eee; 402 | border: 1px solid #ddd; 403 | } 404 | .package-links ul { 405 | padding: 0; 406 | } 407 | .package-links li { 408 | font-size: 10pt; 409 | margin-left: 5px; 410 | padding-left: 0; 411 | list-style: none; 412 | } 413 | #package-links { 414 | /* float this between the content and the Wiki sidebar. there is the issue that large package names can bleed into the right sidebar if they exceed the width */ 415 | float: right; 416 | width: 200px; 417 | padding: 5px; 418 | margin-right: 0; 419 | margin-left: 5px; 420 | background-color: #eee; 421 | border: 1px solid #ddd; 422 | } 423 | #package-links ul { 424 | padding: 0; 425 | } 426 | #package-links li { 427 | font-size: 10pt; 428 | margin-left: 5px; 429 | padding-left: 0; 430 | list-style: none; 431 | } 432 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/seesaw.js: -------------------------------------------------------------------------------- 1 | // seesaw.js for SeeSaw 1.0 2 | function seeSaw(sections,speed,seesaw) { 3 | var dotseesaw = seesaw ? ".seesaw." : "."; 4 | 5 | // Tag toggles 6 | $.each(sections.toggle, function() { 7 | $("span.seesaw." + this + ",div" + dotseesaw + this).each(function() { 8 | if ($(this).css("display") == "none") { 9 | $(this).addClass("seesawshow"); 10 | } 11 | }); 12 | }); 13 | $.each(sections.toggle, function() { 14 | $("span.seesaw." + this + ",div" + dotseesaw + this).each(function() { 15 | if ($(this).css("display") != "none") { 16 | $(this).addClass("seesawhide"); 17 | } 18 | }); 19 | }); 20 | 21 | // Tag shows unless already tagged 22 | $.each(sections.show, function() { 23 | $("span.seesaw." + this).not(".seesawshow,.seesawhide").filter(".hidepart").addClass("seesawshow").end().filter(".showpart").addClass("seesawhide"); 24 | }); 25 | $.each(sections.show, function() { 26 | $("div" + dotseesaw + this).not(".seesawshow,.seesawhide").addClass("seesawshow"); 27 | }); 28 | 29 | // Tag hides unless already tagged 30 | $.each(sections.hide, function() { 31 | $("span.seesaw." + this).not(".seesawshow,.seesawhide").filter(".showpart").addClass("seesawshow").end().filter(".hidepart").addClass("seesawhide"); 32 | }); 33 | $.each(sections.hide, function() { 34 | $("div" + dotseesaw + this).not(".seesawshow,.seesawhide").addClass("seesawhide"); 35 | }); 36 | 37 | // Show or hide according to tag 38 | $(".seesawshow").removeClass("seesawshow").filter("span").show().end().filter("div").show(speed); 39 | $(".seesawhide").removeClass("seesawhide").filter("span").hide().end().filter("div").hide(speed); 40 | } 41 | 42 | $(document).ready(function() { 43 | $("div.seesaw").not(".show").hide(); 44 | $("input.seesaw:hidden").each(function() { 45 | var bg = $(this).attr("value").split(":"); 46 | $("div.seesaw." + bg[0]).css("background-color",bg[1]).removeClass(bg[0]); 47 | }); 48 | }); 49 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/sorttable.js: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/loam_velodyne - ROS Wiki_files/sorttable.js -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/original_src/laserMapping.cpp~: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/original_src/laserMapping.cpp~ -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/original_src/laserOdometry.cpp~: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/original_src/laserOdometry.cpp~ -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/original_src/scanRegistration.cpp~: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/daobilige-su/loam_velodyne/c1133506e3086bd7a4650ca027e6277f58089025/ReadMe_and_supplementary_materials/original_src/scanRegistration.cpp~ -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/original_src/transformMaintenance.cpp: -------------------------------------------------------------------------------- 1 | 00001 #include 2 | 00002 #include 3 | 00003 #include 4 | 00004 #include 5 | 00005 #include 6 | 00006 7 | 00007 #include 8 | 00008 #include 9 | 00009 #include 10 | 00010 11 | 00011 #include 12 | 00012 #include 13 | 00013 14 | 00014 #include 15 | 00015 #include 16 | 00016 17 | 00017 #include 18 | 00018 #include 19 | 00019 #include 20 | 00020 #include 21 | 00021 #include 22 | 00022 23 | 00023 const double PI = 3.1415926; 24 | 00024 const double rad2deg = 180 / PI; 25 | 00025 const double deg2rad = PI / 180; 26 | 00026 27 | 00027 float transformSum[6] = {0}; 28 | 00028 float transformIncre[6] = {0}; 29 | 00029 float transformMapped[6] = {0}; 30 | 00030 float transformBefMapped[6] = {0}; 31 | 00031 float transformAftMapped[6] = {0}; 32 | 00032 33 | 00033 ros::Publisher *pubLaserOdometry2Pointer = NULL; 34 | 00034 tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL; 35 | 00035 nav_msgs::Odometry laserOdometry2; 36 | 00036 tf::StampedTransform laserOdometryTrans2; 37 | 00037 38 | 00038 void transformAssociateToMap() 39 | 00039 { 40 | 00040 float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 41 | 00041 - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 42 | 00042 float y1 = transformBefMapped[4] - transformSum[4]; 43 | 00043 float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 44 | 00044 + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 45 | 00045 46 | 00046 float x2 = x1; 47 | 00047 float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1; 48 | 00048 float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1; 49 | 00049 50 | 00050 transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2; 51 | 00051 transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2; 52 | 00052 transformIncre[5] = z2; 53 | 00053 54 | 00054 float sbcx = sin(transformSum[0]); 55 | 00055 float cbcx = cos(transformSum[0]); 56 | 00056 float sbcy = sin(transformSum[1]); 57 | 00057 float cbcy = cos(transformSum[1]); 58 | 00058 float sbcz = sin(transformSum[2]); 59 | 00059 float cbcz = cos(transformSum[2]); 60 | 00060 61 | 00061 float sblx = sin(transformBefMapped[0]); 62 | 00062 float cblx = cos(transformBefMapped[0]); 63 | 00063 float sbly = sin(transformBefMapped[1]); 64 | 00064 float cbly = cos(transformBefMapped[1]); 65 | 00065 float sblz = sin(transformBefMapped[2]); 66 | 00066 float cblz = cos(transformBefMapped[2]); 67 | 00067 68 | 00068 float salx = sin(transformAftMapped[0]); 69 | 00069 float calx = cos(transformAftMapped[0]); 70 | 00070 float saly = sin(transformAftMapped[1]); 71 | 00071 float caly = cos(transformAftMapped[1]); 72 | 00072 float salz = sin(transformAftMapped[2]); 73 | 00073 float calz = cos(transformAftMapped[2]); 74 | 00074 75 | 00075 float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 76 | 00076 - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 77 | 00077 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 78 | 00078 - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 79 | 00079 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); 80 | 00080 transformMapped[0] = -asin(srx); 81 | 00081 82 | 00082 float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 83 | 00083 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 84 | 00084 - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 85 | 00085 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 86 | 00086 + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 87 | 00087 float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 88 | 00088 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 89 | 00089 - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 90 | 00090 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 91 | 00091 + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 92 | 00092 transformMapped[1] = atan2(srycrx / cos(transformMapped[0]), crycrx / cos(transformMapped[0])); 93 | 00093 94 | 00094 float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 95 | 00095 - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 96 | 00096 - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 97 | 00097 + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 98 | 00098 - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 99 | 00099 + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 100 | 00100 + calx*cblx*salz*sblz); 101 | 00101 float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 102 | 00102 - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 103 | 00103 + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 104 | 00104 + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 105 | 00105 + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 106 | 00106 - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 107 | 00107 - calx*calz*cblx*sblz); 108 | 00108 transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]), crzcrx / cos(transformMapped[0])); 109 | 00109 110 | 00110 x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4]; 111 | 00111 y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4]; 112 | 00112 z1 = transformIncre[5]; 113 | 00113 114 | 00114 x2 = x1; 115 | 00115 y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1; 116 | 00116 z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1; 117 | 00117 118 | 00118 transformMapped[3] = transformAftMapped[3] 119 | 00119 - (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2); 120 | 00120 transformMapped[4] = transformAftMapped[4] - y2; 121 | 00121 transformMapped[5] = transformAftMapped[5] 122 | 00122 - (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2); 123 | 00123 } 124 | 00124 125 | 00125 void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) 126 | 00126 { 127 | 00127 double roll, pitch, yaw; 128 | 00128 geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; 129 | 00129 tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 130 | 00130 131 | 00131 transformSum[0] = -pitch; 132 | 00132 transformSum[1] = -yaw; 133 | 00133 transformSum[2] = roll; 134 | 00134 135 | 00135 transformSum[3] = laserOdometry->pose.pose.position.x; 136 | 00136 transformSum[4] = laserOdometry->pose.pose.position.y; 137 | 00137 transformSum[5] = laserOdometry->pose.pose.position.z; 138 | 00138 139 | 00139 transformAssociateToMap(); 140 | 00140 141 | 00141 geoQuat = tf::createQuaternionMsgFromRollPitchYaw 142 | 00142 (transformMapped[2], -transformMapped[0], -transformMapped[1]); 143 | 00143 144 | 00144 laserOdometry2.header.stamp = laserOdometry->header.stamp; 145 | 00145 laserOdometry2.pose.pose.orientation.x = -geoQuat.y; 146 | 00146 laserOdometry2.pose.pose.orientation.y = -geoQuat.z; 147 | 00147 laserOdometry2.pose.pose.orientation.z = geoQuat.x; 148 | 00148 laserOdometry2.pose.pose.orientation.w = geoQuat.w; 149 | 00149 laserOdometry2.pose.pose.position.x = transformMapped[3]; 150 | 00150 laserOdometry2.pose.pose.position.y = transformMapped[4]; 151 | 00151 laserOdometry2.pose.pose.position.z = transformMapped[5]; 152 | 00152 pubLaserOdometry2Pointer->publish(laserOdometry2); 153 | 00153 154 | 00154 laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; 155 | 00155 laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 156 | 00156 laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5])); 157 | 00157 tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2); 158 | 00158 } 159 | 00159 160 | 00160 void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped) 161 | 00161 { 162 | 00162 double roll, pitch, yaw; 163 | 00163 geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation; 164 | 00164 tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 165 | 00165 166 | 00166 transformAftMapped[0] = -pitch; 167 | 00167 transformAftMapped[1] = -yaw; 168 | 00168 transformAftMapped[2] = roll; 169 | 00169 170 | 00170 transformAftMapped[3] = odomAftMapped->pose.pose.position.x; 171 | 00171 transformAftMapped[4] = odomAftMapped->pose.pose.position.y; 172 | 00172 transformAftMapped[5] = odomAftMapped->pose.pose.position.z; 173 | 00173 174 | 00174 transformBefMapped[0] = odomAftMapped->twist.twist.angular.x; 175 | 00175 transformBefMapped[1] = odomAftMapped->twist.twist.angular.y; 176 | 00176 transformBefMapped[2] = odomAftMapped->twist.twist.angular.z; 177 | 00177 178 | 00178 transformBefMapped[3] = odomAftMapped->twist.twist.linear.x; 179 | 00179 transformBefMapped[4] = odomAftMapped->twist.twist.linear.y; 180 | 00180 transformBefMapped[5] = odomAftMapped->twist.twist.linear.z; 181 | 00181 } 182 | 00182 183 | 00183 int main(int argc, char** argv) 184 | 00184 { 185 | 00185 ros::init(argc, argv, "transformMaintenance"); 186 | 00186 ros::NodeHandle nh; 187 | 00187 188 | 00188 ros::Subscriber subLaserOdometry = nh.subscribe 189 | 00189 ("/laser_odom_to_init", 5, laserOdometryHandler); 190 | 00190 191 | 00191 ros::Subscriber subOdomAftMapped = nh.subscribe 192 | 00192 ("/aft_mapped_to_init", 5, odomAftMappedHandler); 193 | 00193 194 | 00194 ros::Publisher pubLaserOdometry2 = nh.advertise ("/integrated_to_init", 5); 195 | 00195 pubLaserOdometry2Pointer = &pubLaserOdometry2; 196 | 00196 laserOdometry2.header.frame_id = "/camera_init"; 197 | 00197 laserOdometry2.child_frame_id = "/camera"; 198 | 00198 199 | 00199 tf::TransformBroadcaster tfBroadcaster2; 200 | 00200 tfBroadcaster2Pointer = &tfBroadcaster2; 201 | 00201 laserOdometryTrans2.frame_id_ = "/camera_init"; 202 | 00202 laserOdometryTrans2.child_frame_id_ = "/camera"; 203 | 00203 204 | 00204 ros::spin(); 205 | 00205 206 | 00206 return 0; 207 | 00207 } 208 | -------------------------------------------------------------------------------- /ReadMe_and_supplementary_materials/original_src/transformMaintenance.cpp~: -------------------------------------------------------------------------------- 1 | 00001 #include 2 | 00002 #include 3 | 00003 #include 4 | 00004 #include 5 | 00005 #include 6 | 00006 7 | 00007 #include 8 | 00008 #include 9 | 00009 #include 10 | 00010 11 | 00011 #include 12 | 00012 #include 13 | 00013 14 | 00014 #include 15 | 00015 #include 16 | 00016 17 | 00017 #include 18 | 00018 #include 19 | 00019 #include 20 | 00020 #include 21 | 00021 #include 22 | 00022 23 | 00023 const double PI = 3.1415926; 24 | 00024 const double rad2deg = 180 / PI; 25 | 00025 const double deg2rad = PI / 180; 26 | 00026 27 | 00027 float transformSum[6] = {0}; 28 | 00028 float transformIncre[6] = {0}; 29 | 00029 float transformMapped[6] = {0}; 30 | 00030 float transformBefMapped[6] = {0}; 31 | 00031 float transformAftMapped[6] = {0}; 32 | 00032 33 | 00033 ros::Publisher *pubLaserOdometry2Pointer = NULL; 34 | 00034 tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL; 35 | 00035 nav_msgs::Odometry laserOdometry2; 36 | 00036 tf::StampedTransform laserOdometryTrans2; 37 | 00037 38 | 00038 void transformAssociateToMap() 39 | 00039 { 40 | 00040 float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 41 | 00041 - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 42 | 00042 float y1 = transformBefMapped[4] - transformSum[4]; 43 | 00043 float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 44 | 00044 + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 45 | 00045 46 | 00046 float x2 = x1; 47 | 00047 float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1; 48 | 00048 float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1; 49 | 00049 50 | 00050 transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2; 51 | 00051 transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2; 52 | 00052 transformIncre[5] = z2; 53 | 00053 54 | 00054 float sbcx = sin(transformSum[0]); 55 | 00055 float cbcx = cos(transformSum[0]); 56 | 00056 float sbcy = sin(transformSum[1]); 57 | 00057 float cbcy = cos(transformSum[1]); 58 | 00058 float sbcz = sin(transformSum[2]); 59 | 00059 float cbcz = cos(transformSum[2]); 60 | 00060 61 | 00061 float sblx = sin(transformBefMapped[0]); 62 | 00062 float cblx = cos(transformBefMapped[0]); 63 | 00063 float sbly = sin(transformBefMapped[1]); 64 | 00064 float cbly = cos(transformBefMapped[1]); 65 | 00065 float sblz = sin(transformBefMapped[2]); 66 | 00066 float cblz = cos(transformBefMapped[2]); 67 | 00067 68 | 00068 float salx = sin(transformAftMapped[0]); 69 | 00069 float calx = cos(transformAftMapped[0]); 70 | 00070 float saly = sin(transformAftMapped[1]); 71 | 00071 float caly = cos(transformAftMapped[1]); 72 | 00072 float salz = sin(transformAftMapped[2]); 73 | 00073 float calz = cos(transformAftMapped[2]); 74 | 00074 75 | 00075 float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 76 | 00076 - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 77 | 00077 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 78 | 00078 - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 79 | 00079 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); 80 | 00080 transformMapped[0] = -asin(srx); 81 | 00081 82 | 00082 float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 83 | 00083 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 84 | 00084 - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 85 | 00085 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 86 | 00086 + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 87 | 00087 float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 88 | 00088 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 89 | 00089 - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 90 | 00090 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 91 | 00091 + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 92 | 00092 transformMapped[1] = atan2(srycrx / cos(transformMapped[0]), crycrx / cos(transformMapped[0])); 93 | 00093 94 | 00094 float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 95 | 00095 - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 96 | 00096 - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 97 | 00097 + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 98 | 00098 - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 99 | 00099 + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 100 | 00100 + calx*cblx*salz*sblz); 101 | 00101 float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 102 | 00102 - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 103 | 00103 + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 104 | 00104 + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 105 | 00105 + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 106 | 00106 - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 107 | 00107 - calx*calz*cblx*sblz); 108 | 00108 transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]), crzcrx / cos(transformMapped[0])); 109 | 00109 110 | 00110 x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4]; 111 | 00111 y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4]; 112 | 00112 z1 = transformIncre[5]; 113 | 00113 114 | 00114 x2 = x1; 115 | 00115 y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1; 116 | 00116 z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1; 117 | 00117 118 | 00118 transformMapped[3] = transformAftMapped[3] 119 | 00119 - (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2); 120 | 00120 transformMapped[4] = transformAftMapped[4] - y2; 121 | 00121 transformMapped[5] = transformAftMapped[5] 122 | 00122 - (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2); 123 | 00123 } 124 | 00124 125 | 00125 void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) 126 | 00126 { 127 | 00127 double roll, pitch, yaw; 128 | 00128 geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; 129 | 00129 tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 130 | 00130 131 | 00131 transformSum[0] = -pitch; 132 | 00132 transformSum[1] = -yaw; 133 | 00133 transformSum[2] = roll; 134 | 00134 135 | 00135 transformSum[3] = laserOdometry->pose.pose.position.x; 136 | 00136 transformSum[4] = laserOdometry->pose.pose.position.y; 137 | 00137 transformSum[5] = laserOdometry->pose.pose.position.z; 138 | 00138 139 | 00139 transformAssociateToMap(); 140 | 00140 141 | 00141 geoQuat = tf::createQuaternionMsgFromRollPitchYaw 142 | 00142 (transformMapped[2], -transformMapped[0], -transformMapped[1]); 143 | 00143 144 | 00144 laserOdometry2.header.stamp = laserOdometry->header.stamp; 145 | 00145 laserOdometry2.pose.pose.orientation.x = -geoQuat.y; 146 | 00146 laserOdometry2.pose.pose.orientation.y = -geoQuat.z; 147 | 00147 laserOdometry2.pose.pose.orientation.z = geoQuat.x; 148 | 00148 laserOdometry2.pose.pose.orientation.w = geoQuat.w; 149 | 00149 laserOdometry2.pose.pose.position.x = transformMapped[3]; 150 | 00150 laserOdometry2.pose.pose.position.y = transformMapped[4]; 151 | 00151 laserOdometry2.pose.pose.position.z = transformMapped[5]; 152 | 00152 pubLaserOdometry2Pointer->publish(laserOdometry2); 153 | 00153 154 | 00154 laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; 155 | 00155 laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 156 | 00156 laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5])); 157 | 00157 tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2); 158 | 00158 } 159 | 00159 160 | 00160 void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped) 161 | 00161 { 162 | 00162 double roll, pitch, yaw; 163 | 00163 geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation; 164 | 00164 tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 165 | 00165 166 | 00166 transformAftMapped[0] = -pitch; 167 | 00167 transformAftMapped[1] = -yaw; 168 | 00168 transformAftMapped[2] = roll; 169 | 00169 170 | 00170 transformAftMapped[3] = odomAftMapped->pose.pose.position.x; 171 | 00171 transformAftMapped[4] = odomAftMapped->pose.pose.position.y; 172 | 00172 transformAftMapped[5] = odomAftMapped->pose.pose.position.z; 173 | 00173 174 | 00174 transformBefMapped[0] = odomAftMapped->twist.twist.angular.x; 175 | 00175 transformBefMapped[1] = odomAftMapped->twist.twist.angular.y; 176 | 00176 transformBefMapped[2] = odomAftMapped->twist.twist.angular.z; 177 | 00177 178 | 00178 transformBefMapped[3] = odomAftMapped->twist.twist.linear.x; 179 | 00179 transformBefMapped[4] = odomAftMapped->twist.twist.linear.y; 180 | 00180 transformBefMapped[5] = odomAftMapped->twist.twist.linear.z; 181 | 00181 } 182 | 00182 183 | 00183 int main(int argc, char** argv) 184 | 00184 { 185 | 00185 ros::init(argc, argv, "transformMaintenance"); 186 | 00186 ros::NodeHandle nh; 187 | 00187 188 | 00188 ros::Subscriber subLaserOdometry = nh.subscribe 189 | 00189 ("/laser_odom_to_init", 5, laserOdometryHandler); 190 | 00190 191 | 00191 ros::Subscriber subOdomAftMapped = nh.subscribe 192 | 00192 ("/aft_mapped_to_init", 5, odomAftMappedHandler); 193 | 00193 194 | 00194 ros::Publisher pubLaserOdometry2 = nh.advertise ("/integrated_to_init", 5); 195 | 00195 pubLaserOdometry2Pointer = &pubLaserOdometry2; 196 | 00196 laserOdometry2.header.frame_id = "/camera_init"; 197 | 00197 laserOdometry2.child_frame_id = "/camera"; 198 | 00198 199 | 00199 tf::TransformBroadcaster tfBroadcaster2; 200 | 00200 tfBroadcaster2Pointer = &tfBroadcaster2; 201 | 00201 laserOdometryTrans2.frame_id_ = "/camera_init"; 202 | 00202 laserOdometryTrans2.child_frame_id_ = "/camera"; 203 | 00203 204 | 00204 ros::spin(); 205 | 00205 206 | 00206 return 0; 207 | 00207 } 208 | -------------------------------------------------------------------------------- /include/loam_velodyne/common.h: -------------------------------------------------------------------------------- 1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 2 | // Further contributions copyright (c) 2016, Southwest Research Institute 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // 1. Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived from this 15 | // software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // This is an implementation of the algorithm described in the following paper: 30 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 31 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 32 | 33 | #include 34 | 35 | #include 36 | 37 | typedef pcl::PointXYZI PointType; 38 | 39 | inline double rad2deg(double radians) 40 | { 41 | return radians * 180.0 / M_PI; 42 | } 43 | 44 | inline double deg2rad(double degrees) 45 | { 46 | return degrees * M_PI / 180.0; 47 | } 48 | -------------------------------------------------------------------------------- /launch/hector_loam_velodyne.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /launch/loam_velodyne.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | loam_velodyne 4 | 0.0.0 5 | 6 | 7 | This is a ROS implementation of LOAM for Velodyne multiplane laser scanners. 8 | LOAM is described in the following paper: 9 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 10 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 11 | 12 | 13 | Ed Venator 14 | 15 | BSD 16 | 17 | Ji Zhang 18 | 19 | catkin 20 | geometry_msgs 21 | nav_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | sensor_msgs 26 | tf 27 | 28 | geometry_msgs 29 | nav_msgs 30 | sensor_msgs 31 | roscpp 32 | rospy 33 | std_msgs 34 | tf 35 | 36 | rostest 37 | rosbag 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /rviz_cfg/loam_velodyne.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 | - /Odometry1 10 | - /PointCloud21 11 | - /Odometry2 12 | - /PointCloud22 13 | Splitter Ratio: 0.5 14 | Tree Height: 745 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.588679 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: PointCloud2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.03 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/TF 56 | Enabled: true 57 | Frame Timeout: 15 58 | Frames: 59 | All Enabled: true 60 | aft_mapped: 61 | Value: true 62 | camera: 63 | Value: true 64 | camera_init: 65 | Value: true 66 | laser_odom: 67 | Value: true 68 | Marker Scale: 1 69 | Name: TF 70 | Show Arrows: true 71 | Show Axes: true 72 | Show Names: true 73 | Tree: 74 | camera_init: 75 | aft_mapped: 76 | {} 77 | camera: 78 | {} 79 | laser_odom: 80 | {} 81 | Update Interval: 0 82 | Value: true 83 | - Angle Tolerance: 0.1 84 | Class: rviz/Odometry 85 | Color: 255; 25; 0 86 | Enabled: true 87 | Keep: 100 88 | Length: 1 89 | Name: Odometry 90 | Position Tolerance: 0.1 91 | Topic: /integrated_to_init 92 | Value: true 93 | - Alpha: 1 94 | Autocompute Intensity Bounds: true 95 | Autocompute Value Bounds: 96 | Max Value: 48.3273 97 | Min Value: -11.1357 98 | Value: true 99 | Axis: Y 100 | Channel Name: intensity 101 | Class: rviz/PointCloud2 102 | Color: 255; 255; 255 103 | Color Transformer: FlatColor 104 | Decay Time: 0 105 | Enabled: true 106 | Invert Rainbow: false 107 | Max Color: 255; 255; 255 108 | Max Intensity: 15 109 | Min Color: 0; 0; 0 110 | Min Intensity: 0 111 | Name: PointCloud2 112 | Position Transformer: XYZ 113 | Queue Size: 10 114 | Selectable: true 115 | Size (Pixels): 1 116 | Size (m): 0.01 117 | Style: Points 118 | Topic: /laser_cloud_surround 119 | Use Fixed Frame: true 120 | Use rainbow: true 121 | Value: true 122 | - Angle Tolerance: 0.1 123 | Class: rviz/Odometry 124 | Color: 0; 170; 0 125 | Enabled: false 126 | Keep: 100 127 | Length: 1 128 | Name: Odometry 129 | Position Tolerance: 0.1 130 | Topic: /laser_odom_to_init 131 | Value: false 132 | - Alpha: 1 133 | Autocompute Intensity Bounds: true 134 | Autocompute Value Bounds: 135 | Max Value: 10 136 | Min Value: -10 137 | Value: true 138 | Axis: Z 139 | Channel Name: intensity 140 | Class: rviz/PointCloud2 141 | Color: 255; 255; 255 142 | Color Transformer: Intensity 143 | Decay Time: 0 144 | Enabled: true 145 | Invert Rainbow: false 146 | Max Color: 255; 255; 255 147 | Max Intensity: 15 148 | Min Color: 0; 0; 0 149 | Min Intensity: 0 150 | Name: PointCloud2 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: /velodyne_cloud_registered 158 | Use Fixed Frame: true 159 | Use rainbow: true 160 | Value: true 161 | Enabled: true 162 | Global Options: 163 | Background Color: 48; 48; 48 164 | Fixed Frame: camera_init 165 | Frame Rate: 30 166 | Name: root 167 | Tools: 168 | - Class: rviz/Interact 169 | Hide Inactive Objects: true 170 | - Class: rviz/MoveCamera 171 | - Class: rviz/Select 172 | - Class: rviz/FocusCamera 173 | - Class: rviz/Measure 174 | - Class: rviz/SetInitialPose 175 | Topic: /initialpose 176 | - Class: rviz/SetGoal 177 | Topic: /move_base_simple/goal 178 | - Class: rviz/PublishPoint 179 | Single click: true 180 | Topic: /clicked_point 181 | Value: true 182 | Views: 183 | Current: 184 | Class: rviz/ThirdPersonFollower 185 | Distance: 228.107 186 | Enable Stereo Rendering: 187 | Stereo Eye Separation: 0.06 188 | Stereo Focal Distance: 1 189 | Swap Stereo Eyes: false 190 | Value: false 191 | Focal Point: 192 | X: -5.59986 193 | Y: -75.2008 194 | Z: 0 195 | Name: Current View 196 | Near Clip Distance: 0.01 197 | Pitch: -1.1048 198 | Target Frame: aft_mapped 199 | Value: ThirdPersonFollower (rviz) 200 | Yaw: 1.65038 201 | Saved: ~ 202 | Window Geometry: 203 | Displays: 204 | collapsed: false 205 | Height: 1026 206 | Hide Left Dock: false 207 | Hide Right Dock: false 208 | QMainWindow State: 000000ff00000000fd00000004000000000000013c00000378fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000378000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000378fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000378000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077a0000003efc0100000002fb0000000800540069006d006501000000000000077a000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005230000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 209 | Selection: 210 | collapsed: false 211 | Time: 212 | collapsed: false 213 | Tool Properties: 214 | collapsed: false 215 | Views: 216 | collapsed: false 217 | Width: 1914 218 | X: 636 219 | Y: 247 220 | -------------------------------------------------------------------------------- /src/scanRegistration.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 2 | // Further contributions copyright (c) 2016, Southwest Research Institute 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // 1. Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived from this 15 | // software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // This is an implementation of the algorithm described in the following paper: 30 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 31 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | using std::sin; 52 | using std::cos; 53 | using std::atan2; 54 | 55 | const double scanPeriod = 0.1; 56 | 57 | const int systemDelay = 20; 58 | int systemInitCount = 0; 59 | bool systemInited = false; 60 | 61 | const int N_SCANS = 16; 62 | 63 | float cloudCurvature[40000]; 64 | int cloudSortInd[40000]; 65 | int cloudNeighborPicked[40000]; 66 | int cloudLabel[40000]; 67 | 68 | int imuPointerFront = 0; 69 | int imuPointerLast = -1; 70 | const int imuQueLength = 200; 71 | 72 | float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0; 73 | float imuRollCur = 0, imuPitchCur = 0, imuYawCur = 0; 74 | 75 | float imuVeloXStart = 0, imuVeloYStart = 0, imuVeloZStart = 0; 76 | float imuShiftXStart = 0, imuShiftYStart = 0, imuShiftZStart = 0; 77 | 78 | float imuVeloXCur = 0, imuVeloYCur = 0, imuVeloZCur = 0; 79 | float imuShiftXCur = 0, imuShiftYCur = 0, imuShiftZCur = 0; 80 | 81 | float imuShiftFromStartXCur = 0, imuShiftFromStartYCur = 0, imuShiftFromStartZCur = 0; 82 | float imuVeloFromStartXCur = 0, imuVeloFromStartYCur = 0, imuVeloFromStartZCur = 0; 83 | 84 | double imuTime[imuQueLength] = {0}; 85 | float imuRoll[imuQueLength] = {0}; 86 | float imuPitch[imuQueLength] = {0}; 87 | float imuYaw[imuQueLength] = {0}; 88 | 89 | float imuAccX[imuQueLength] = {0}; 90 | float imuAccY[imuQueLength] = {0}; 91 | float imuAccZ[imuQueLength] = {0}; 92 | 93 | float imuVeloX[imuQueLength] = {0}; 94 | float imuVeloY[imuQueLength] = {0}; 95 | float imuVeloZ[imuQueLength] = {0}; 96 | 97 | float imuShiftX[imuQueLength] = {0}; 98 | float imuShiftY[imuQueLength] = {0}; 99 | float imuShiftZ[imuQueLength] = {0}; 100 | 101 | ros::Publisher pubLaserCloud; 102 | ros::Publisher pubCornerPointsSharp; 103 | ros::Publisher pubCornerPointsLessSharp; 104 | ros::Publisher pubSurfPointsFlat; 105 | ros::Publisher pubSurfPointsLessFlat; 106 | ros::Publisher pubImuTrans; 107 | 108 | void ShiftToStartIMU(float pointTime) 109 | { 110 | imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime; 111 | imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime; 112 | imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime; 113 | 114 | float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur; 115 | float y1 = imuShiftFromStartYCur; 116 | float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur; 117 | 118 | float x2 = x1; 119 | float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1; 120 | float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1; 121 | 122 | imuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2; 123 | imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2; 124 | imuShiftFromStartZCur = z2; 125 | } 126 | 127 | void VeloToStartIMU() 128 | { 129 | imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart; 130 | imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart; 131 | imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart; 132 | 133 | float x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur; 134 | float y1 = imuVeloFromStartYCur; 135 | float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur; 136 | 137 | float x2 = x1; 138 | float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1; 139 | float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1; 140 | 141 | imuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2; 142 | imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2; 143 | imuVeloFromStartZCur = z2; 144 | } 145 | 146 | void TransformToStartIMU(PointType *p) 147 | { 148 | float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y; 149 | float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y; 150 | float z1 = p->z; 151 | 152 | float x2 = x1; 153 | float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1; 154 | float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1; 155 | 156 | float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2; 157 | float y3 = y2; 158 | float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2; 159 | 160 | float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3; 161 | float y4 = y3; 162 | float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3; 163 | 164 | float x5 = x4; 165 | float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4; 166 | float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4; 167 | 168 | p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur; 169 | p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur; 170 | p->z = z5 + imuShiftFromStartZCur; 171 | } 172 | 173 | void AccumulateIMUShift() 174 | { 175 | float roll = imuRoll[imuPointerLast]; 176 | float pitch = imuPitch[imuPointerLast]; 177 | float yaw = imuYaw[imuPointerLast]; 178 | float accX = imuAccX[imuPointerLast]; 179 | float accY = imuAccY[imuPointerLast]; 180 | float accZ = imuAccZ[imuPointerLast]; 181 | 182 | float x1 = cos(roll) * accX - sin(roll) * accY; 183 | float y1 = sin(roll) * accX + cos(roll) * accY; 184 | float z1 = accZ; 185 | 186 | float x2 = x1; 187 | float y2 = cos(pitch) * y1 - sin(pitch) * z1; 188 | float z2 = sin(pitch) * y1 + cos(pitch) * z1; 189 | 190 | accX = cos(yaw) * x2 + sin(yaw) * z2; 191 | accY = y2; 192 | accZ = -sin(yaw) * x2 + cos(yaw) * z2; 193 | 194 | int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength; 195 | double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack]; 196 | if (timeDiff < scanPeriod) { 197 | 198 | imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff 199 | + accX * timeDiff * timeDiff / 2; 200 | imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff 201 | + accY * timeDiff * timeDiff / 2; 202 | imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff 203 | + accZ * timeDiff * timeDiff / 2; 204 | 205 | imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff; 206 | imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff; 207 | imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff; 208 | } 209 | } 210 | 211 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) 212 | { 213 | if (!systemInited) { 214 | systemInitCount++; 215 | if (systemInitCount >= systemDelay) { 216 | systemInited = true; 217 | } 218 | return; 219 | } 220 | 221 | std::vector scanStartInd(N_SCANS, 0); 222 | std::vector scanEndInd(N_SCANS, 0); 223 | 224 | double timeScanCur = laserCloudMsg->header.stamp.toSec(); 225 | pcl::PointCloud laserCloudIn; 226 | pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); 227 | std::vector indices; 228 | pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); 229 | int cloudSize = laserCloudIn.points.size(); 230 | float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); 231 | float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, 232 | laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI; 233 | 234 | if (endOri - startOri > 3 * M_PI) { 235 | endOri -= 2 * M_PI; 236 | } else if (endOri - startOri < M_PI) { 237 | endOri += 2 * M_PI; 238 | } 239 | bool halfPassed = false; 240 | int count = cloudSize; 241 | PointType point; 242 | std::vector > laserCloudScans(N_SCANS); 243 | for (int i = 0; i < cloudSize; i++) { 244 | point.x = laserCloudIn.points[i].y; 245 | point.y = laserCloudIn.points[i].z; 246 | point.z = laserCloudIn.points[i].x; 247 | 248 | float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI; 249 | int scanID; 250 | int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); 251 | if (roundedAngle > 0){ 252 | scanID = roundedAngle; 253 | } 254 | else { 255 | scanID = roundedAngle + (N_SCANS - 1); 256 | } 257 | if (scanID > (N_SCANS - 1) || scanID < 0 ){ 258 | count--; 259 | continue; 260 | } 261 | 262 | float ori = -atan2(point.x, point.z); 263 | if (!halfPassed) { 264 | if (ori < startOri - M_PI / 2) { 265 | ori += 2 * M_PI; 266 | } else if (ori > startOri + M_PI * 3 / 2) { 267 | ori -= 2 * M_PI; 268 | } 269 | 270 | if (ori - startOri > M_PI) { 271 | halfPassed = true; 272 | } 273 | } else { 274 | ori += 2 * M_PI; 275 | 276 | if (ori < endOri - M_PI * 3 / 2) { 277 | ori += 2 * M_PI; 278 | } else if (ori > endOri + M_PI / 2) { 279 | ori -= 2 * M_PI; 280 | } 281 | } 282 | 283 | float relTime = (ori - startOri) / (endOri - startOri); 284 | point.intensity = scanID + scanPeriod * relTime; 285 | 286 | if (imuPointerLast >= 0) { 287 | float pointTime = relTime * scanPeriod; 288 | while (imuPointerFront != imuPointerLast) { 289 | if (timeScanCur + pointTime < imuTime[imuPointerFront]) { 290 | break; 291 | } 292 | imuPointerFront = (imuPointerFront + 1) % imuQueLength; 293 | } 294 | 295 | if (timeScanCur + pointTime > imuTime[imuPointerFront]) { 296 | imuRollCur = imuRoll[imuPointerFront]; 297 | imuPitchCur = imuPitch[imuPointerFront]; 298 | imuYawCur = imuYaw[imuPointerFront]; 299 | 300 | imuVeloXCur = imuVeloX[imuPointerFront]; 301 | imuVeloYCur = imuVeloY[imuPointerFront]; 302 | imuVeloZCur = imuVeloZ[imuPointerFront]; 303 | 304 | imuShiftXCur = imuShiftX[imuPointerFront]; 305 | imuShiftYCur = imuShiftY[imuPointerFront]; 306 | imuShiftZCur = imuShiftZ[imuPointerFront]; 307 | } else { 308 | int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength; 309 | float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) 310 | / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); 311 | float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) 312 | / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); 313 | 314 | imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack; 315 | imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack; 316 | if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) { 317 | imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack; 318 | } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) { 319 | imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack; 320 | } else { 321 | imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack; 322 | } 323 | 324 | imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack; 325 | imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack; 326 | imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack; 327 | 328 | imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack; 329 | imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack; 330 | imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack; 331 | } 332 | if (i == 0) { 333 | imuRollStart = imuRollCur; 334 | imuPitchStart = imuPitchCur; 335 | imuYawStart = imuYawCur; 336 | 337 | imuVeloXStart = imuVeloXCur; 338 | imuVeloYStart = imuVeloYCur; 339 | imuVeloZStart = imuVeloZCur; 340 | 341 | imuShiftXStart = imuShiftXCur; 342 | imuShiftYStart = imuShiftYCur; 343 | imuShiftZStart = imuShiftZCur; 344 | } else { 345 | ShiftToStartIMU(pointTime); 346 | VeloToStartIMU(); 347 | TransformToStartIMU(&point); 348 | } 349 | } 350 | laserCloudScans[scanID].push_back(point); 351 | } 352 | cloudSize = count; 353 | 354 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 355 | for (int i = 0; i < N_SCANS; i++) { 356 | *laserCloud += laserCloudScans[i]; 357 | } 358 | int scanCount = -1; 359 | for (int i = 5; i < cloudSize - 5; i++) { 360 | float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x 361 | + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x 362 | + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x 363 | + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x 364 | + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x 365 | + laserCloud->points[i + 5].x; 366 | float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y 367 | + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y 368 | + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y 369 | + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y 370 | + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y 371 | + laserCloud->points[i + 5].y; 372 | float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z 373 | + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z 374 | + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z 375 | + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z 376 | + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z 377 | + laserCloud->points[i + 5].z; 378 | cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ; 379 | cloudSortInd[i] = i; 380 | cloudNeighborPicked[i] = 0; 381 | cloudLabel[i] = 0; 382 | 383 | if (int(laserCloud->points[i].intensity) != scanCount) { 384 | scanCount = int(laserCloud->points[i].intensity); 385 | 386 | if (scanCount > 0 && scanCount < N_SCANS) { 387 | scanStartInd[scanCount] = i + 5; 388 | scanEndInd[scanCount - 1] = i - 5; 389 | } 390 | } 391 | } 392 | scanStartInd[0] = 5; 393 | scanEndInd.back() = cloudSize - 5; 394 | 395 | for (int i = 5; i < cloudSize - 6; i++) { 396 | float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x; 397 | float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y; 398 | float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z; 399 | float diff = diffX * diffX + diffY * diffY + diffZ * diffZ; 400 | 401 | if (diff > 0.1) { 402 | 403 | float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + 404 | laserCloud->points[i].y * laserCloud->points[i].y + 405 | laserCloud->points[i].z * laserCloud->points[i].z); 406 | 407 | float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + 408 | laserCloud->points[i + 1].y * laserCloud->points[i + 1].y + 409 | laserCloud->points[i + 1].z * laserCloud->points[i + 1].z); 410 | 411 | if (depth1 > depth2) { 412 | diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1; 413 | diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1; 414 | diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1; 415 | 416 | if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) { 417 | cloudNeighborPicked[i - 5] = 1; 418 | cloudNeighborPicked[i - 4] = 1; 419 | cloudNeighborPicked[i - 3] = 1; 420 | cloudNeighborPicked[i - 2] = 1; 421 | cloudNeighborPicked[i - 1] = 1; 422 | cloudNeighborPicked[i] = 1; 423 | } 424 | } else { 425 | diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x; 426 | diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y; 427 | diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z; 428 | 429 | if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) { 430 | cloudNeighborPicked[i + 1] = 1; 431 | cloudNeighborPicked[i + 2] = 1; 432 | cloudNeighborPicked[i + 3] = 1; 433 | cloudNeighborPicked[i + 4] = 1; 434 | cloudNeighborPicked[i + 5] = 1; 435 | cloudNeighborPicked[i + 6] = 1; 436 | } 437 | } 438 | } 439 | 440 | float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x; 441 | float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y; 442 | float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z; 443 | float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2; 444 | 445 | float dis = laserCloud->points[i].x * laserCloud->points[i].x 446 | + laserCloud->points[i].y * laserCloud->points[i].y 447 | + laserCloud->points[i].z * laserCloud->points[i].z; 448 | 449 | if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) { 450 | cloudNeighborPicked[i] = 1; 451 | } 452 | } 453 | 454 | 455 | pcl::PointCloud cornerPointsSharp; 456 | pcl::PointCloud cornerPointsLessSharp; 457 | pcl::PointCloud surfPointsFlat; 458 | pcl::PointCloud surfPointsLessFlat; 459 | 460 | for (int i = 0; i < N_SCANS; i++) { 461 | pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); 462 | for (int j = 0; j < 6; j++) { 463 | int sp = (scanStartInd[i] * (6 - j) + scanEndInd[i] * j) / 6; 464 | int ep = (scanStartInd[i] * (5 - j) + scanEndInd[i] * (j + 1)) / 6 - 1; 465 | 466 | for (int k = sp + 1; k <= ep; k++) { 467 | for (int l = k; l >= sp + 1; l--) { 468 | if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) { 469 | int temp = cloudSortInd[l - 1]; 470 | cloudSortInd[l - 1] = cloudSortInd[l]; 471 | cloudSortInd[l] = temp; 472 | } 473 | } 474 | } 475 | 476 | int largestPickedNum = 0; 477 | for (int k = ep; k >= sp; k--) { 478 | int ind = cloudSortInd[k]; 479 | if (cloudNeighborPicked[ind] == 0 && 480 | cloudCurvature[ind] > 0.1) { 481 | 482 | largestPickedNum++; 483 | if (largestPickedNum <= 2) { 484 | cloudLabel[ind] = 2; 485 | cornerPointsSharp.push_back(laserCloud->points[ind]); 486 | cornerPointsLessSharp.push_back(laserCloud->points[ind]); 487 | } else if (largestPickedNum <= 20) { 488 | cloudLabel[ind] = 1; 489 | cornerPointsLessSharp.push_back(laserCloud->points[ind]); 490 | } else { 491 | break; 492 | } 493 | 494 | cloudNeighborPicked[ind] = 1; 495 | for (int l = 1; l <= 5; l++) { 496 | float diffX = laserCloud->points[ind + l].x 497 | - laserCloud->points[ind + l - 1].x; 498 | float diffY = laserCloud->points[ind + l].y 499 | - laserCloud->points[ind + l - 1].y; 500 | float diffZ = laserCloud->points[ind + l].z 501 | - laserCloud->points[ind + l - 1].z; 502 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { 503 | break; 504 | } 505 | 506 | cloudNeighborPicked[ind + l] = 1; 507 | } 508 | for (int l = -1; l >= -5; l--) { 509 | float diffX = laserCloud->points[ind + l].x 510 | - laserCloud->points[ind + l + 1].x; 511 | float diffY = laserCloud->points[ind + l].y 512 | - laserCloud->points[ind + l + 1].y; 513 | float diffZ = laserCloud->points[ind + l].z 514 | - laserCloud->points[ind + l + 1].z; 515 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { 516 | break; 517 | } 518 | 519 | cloudNeighborPicked[ind + l] = 1; 520 | } 521 | } 522 | } 523 | 524 | int smallestPickedNum = 0; 525 | for (int k = sp; k <= ep; k++) { 526 | int ind = cloudSortInd[k]; 527 | if (cloudNeighborPicked[ind] == 0 && 528 | cloudCurvature[ind] < 0.1) { 529 | 530 | cloudLabel[ind] = -1; 531 | surfPointsFlat.push_back(laserCloud->points[ind]); 532 | 533 | smallestPickedNum++; 534 | if (smallestPickedNum >= 4) { 535 | break; 536 | } 537 | 538 | cloudNeighborPicked[ind] = 1; 539 | for (int l = 1; l <= 5; l++) { 540 | float diffX = laserCloud->points[ind + l].x 541 | - laserCloud->points[ind + l - 1].x; 542 | float diffY = laserCloud->points[ind + l].y 543 | - laserCloud->points[ind + l - 1].y; 544 | float diffZ = laserCloud->points[ind + l].z 545 | - laserCloud->points[ind + l - 1].z; 546 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { 547 | break; 548 | } 549 | 550 | cloudNeighborPicked[ind + l] = 1; 551 | } 552 | for (int l = -1; l >= -5; l--) { 553 | float diffX = laserCloud->points[ind + l].x 554 | - laserCloud->points[ind + l + 1].x; 555 | float diffY = laserCloud->points[ind + l].y 556 | - laserCloud->points[ind + l + 1].y; 557 | float diffZ = laserCloud->points[ind + l].z 558 | - laserCloud->points[ind + l + 1].z; 559 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) { 560 | break; 561 | } 562 | 563 | cloudNeighborPicked[ind + l] = 1; 564 | } 565 | } 566 | } 567 | 568 | for (int k = sp; k <= ep; k++) { 569 | if (cloudLabel[k] <= 0) { 570 | surfPointsLessFlatScan->push_back(laserCloud->points[k]); 571 | } 572 | } 573 | } 574 | 575 | pcl::PointCloud surfPointsLessFlatScanDS; 576 | pcl::VoxelGrid downSizeFilter; 577 | downSizeFilter.setInputCloud(surfPointsLessFlatScan); 578 | downSizeFilter.setLeafSize(0.2, 0.2, 0.2); 579 | downSizeFilter.filter(surfPointsLessFlatScanDS); 580 | 581 | surfPointsLessFlat += surfPointsLessFlatScanDS; 582 | } 583 | 584 | sensor_msgs::PointCloud2 laserCloudOutMsg; 585 | pcl::toROSMsg(*laserCloud, laserCloudOutMsg); 586 | laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; 587 | laserCloudOutMsg.header.frame_id = "/camera"; 588 | pubLaserCloud.publish(laserCloudOutMsg); 589 | 590 | sensor_msgs::PointCloud2 cornerPointsSharpMsg; 591 | pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg); 592 | cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp; 593 | cornerPointsSharpMsg.header.frame_id = "/camera"; 594 | pubCornerPointsSharp.publish(cornerPointsSharpMsg); 595 | 596 | sensor_msgs::PointCloud2 cornerPointsLessSharpMsg; 597 | pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg); 598 | cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp; 599 | cornerPointsLessSharpMsg.header.frame_id = "/camera"; 600 | pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg); 601 | 602 | sensor_msgs::PointCloud2 surfPointsFlat2; 603 | pcl::toROSMsg(surfPointsFlat, surfPointsFlat2); 604 | surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp; 605 | surfPointsFlat2.header.frame_id = "/camera"; 606 | pubSurfPointsFlat.publish(surfPointsFlat2); 607 | 608 | sensor_msgs::PointCloud2 surfPointsLessFlat2; 609 | pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2); 610 | surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp; 611 | surfPointsLessFlat2.header.frame_id = "/camera"; 612 | pubSurfPointsLessFlat.publish(surfPointsLessFlat2); 613 | 614 | pcl::PointCloud imuTrans(4, 1); 615 | imuTrans.points[0].x = imuPitchStart; 616 | imuTrans.points[0].y = imuYawStart; 617 | imuTrans.points[0].z = imuRollStart; 618 | 619 | imuTrans.points[1].x = imuPitchCur; 620 | imuTrans.points[1].y = imuYawCur; 621 | imuTrans.points[1].z = imuRollCur; 622 | 623 | imuTrans.points[2].x = imuShiftFromStartXCur; 624 | imuTrans.points[2].y = imuShiftFromStartYCur; 625 | imuTrans.points[2].z = imuShiftFromStartZCur; 626 | 627 | imuTrans.points[3].x = imuVeloFromStartXCur; 628 | imuTrans.points[3].y = imuVeloFromStartYCur; 629 | imuTrans.points[3].z = imuVeloFromStartZCur; 630 | 631 | sensor_msgs::PointCloud2 imuTransMsg; 632 | pcl::toROSMsg(imuTrans, imuTransMsg); 633 | imuTransMsg.header.stamp = laserCloudMsg->header.stamp; 634 | imuTransMsg.header.frame_id = "/camera"; 635 | pubImuTrans.publish(imuTransMsg); 636 | } 637 | 638 | void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) 639 | { 640 | double roll, pitch, yaw; 641 | tf::Quaternion orientation; 642 | tf::quaternionMsgToTF(imuIn->orientation, orientation); 643 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); 644 | 645 | float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81; 646 | float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81; 647 | float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81; 648 | 649 | imuPointerLast = (imuPointerLast + 1) % imuQueLength; 650 | 651 | imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); 652 | imuRoll[imuPointerLast] = roll; 653 | imuPitch[imuPointerLast] = pitch; 654 | imuYaw[imuPointerLast] = yaw; 655 | imuAccX[imuPointerLast] = accX; 656 | imuAccY[imuPointerLast] = accY; 657 | imuAccZ[imuPointerLast] = accZ; 658 | 659 | AccumulateIMUShift(); 660 | } 661 | 662 | int main(int argc, char** argv) 663 | { 664 | //ros::init(argc, argv, "scanRegistration"); 665 | ros::init(argc, argv, "scanRegistration"); 666 | ros::NodeHandle nh; 667 | 668 | ros::Subscriber subLaserCloud = nh.subscribe 669 | ("/velodyne_points", 2, laserCloudHandler); 670 | 671 | ros::Subscriber subImu = nh.subscribe ("/imu/data", 50, imuHandler); 672 | 673 | pubLaserCloud = nh.advertise 674 | ("/velodyne_cloud_2", 2); 675 | 676 | pubCornerPointsSharp = nh.advertise 677 | ("/laser_cloud_sharp", 2); 678 | 679 | pubCornerPointsLessSharp = nh.advertise 680 | ("/laser_cloud_less_sharp", 2); 681 | 682 | pubSurfPointsFlat = nh.advertise 683 | ("/laser_cloud_flat", 2); 684 | 685 | pubSurfPointsLessFlat = nh.advertise 686 | ("/laser_cloud_less_flat", 2); 687 | 688 | pubImuTrans = nh.advertise ("/imu_trans", 5); 689 | 690 | ros::spin(); 691 | 692 | return 0; 693 | } 694 | 695 | -------------------------------------------------------------------------------- /src/transformMaintenance.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 2 | // Further contributions copyright (c) 2016, Southwest Research Institute 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // 1. Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived from this 15 | // software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // This is an implementation of the algorithm described in the following paper: 30 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 31 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | float transformSum[6] = {0}; 50 | float transformIncre[6] = {0}; 51 | float transformMapped[6] = {0}; 52 | float transformBefMapped[6] = {0}; 53 | float transformAftMapped[6] = {0}; 54 | 55 | ros::Publisher *pubLaserOdometry2Pointer = NULL; 56 | tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL; 57 | nav_msgs::Odometry laserOdometry2; 58 | tf::StampedTransform laserOdometryTrans2; 59 | 60 | void transformAssociateToMap() 61 | { 62 | float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 63 | - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 64 | float y1 = transformBefMapped[4] - transformSum[4]; 65 | float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 66 | + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 67 | 68 | float x2 = x1; 69 | float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1; 70 | float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1; 71 | 72 | transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2; 73 | transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2; 74 | transformIncre[5] = z2; 75 | 76 | float sbcx = sin(transformSum[0]); 77 | float cbcx = cos(transformSum[0]); 78 | float sbcy = sin(transformSum[1]); 79 | float cbcy = cos(transformSum[1]); 80 | float sbcz = sin(transformSum[2]); 81 | float cbcz = cos(transformSum[2]); 82 | 83 | float sblx = sin(transformBefMapped[0]); 84 | float cblx = cos(transformBefMapped[0]); 85 | float sbly = sin(transformBefMapped[1]); 86 | float cbly = cos(transformBefMapped[1]); 87 | float sblz = sin(transformBefMapped[2]); 88 | float cblz = cos(transformBefMapped[2]); 89 | 90 | float salx = sin(transformAftMapped[0]); 91 | float calx = cos(transformAftMapped[0]); 92 | float saly = sin(transformAftMapped[1]); 93 | float caly = cos(transformAftMapped[1]); 94 | float salz = sin(transformAftMapped[2]); 95 | float calz = cos(transformAftMapped[2]); 96 | 97 | float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 98 | - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 99 | - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 100 | - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 101 | - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); 102 | transformMapped[0] = -asin(srx); 103 | 104 | float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 105 | - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 106 | - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 107 | - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 108 | + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 109 | float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 110 | - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 111 | - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 112 | - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 113 | + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 114 | transformMapped[1] = atan2(srycrx / cos(transformMapped[0]), crycrx / cos(transformMapped[0])); 115 | 116 | float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 117 | - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 118 | - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 119 | + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 120 | - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 121 | + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 122 | + calx*cblx*salz*sblz); 123 | float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 124 | - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 125 | + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 126 | + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 127 | + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 128 | - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 129 | - calx*calz*cblx*sblz); 130 | transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]), crzcrx / cos(transformMapped[0])); 131 | 132 | x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4]; 133 | y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4]; 134 | z1 = transformIncre[5]; 135 | 136 | x2 = x1; 137 | y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1; 138 | z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1; 139 | 140 | transformMapped[3] = transformAftMapped[3] 141 | - (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2); 142 | transformMapped[4] = transformAftMapped[4] - y2; 143 | transformMapped[5] = transformAftMapped[5] 144 | - (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2); 145 | } 146 | 147 | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) 148 | { 149 | double roll, pitch, yaw; 150 | geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; 151 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 152 | 153 | transformSum[0] = -pitch; 154 | transformSum[1] = -yaw; 155 | transformSum[2] = roll; 156 | 157 | transformSum[3] = laserOdometry->pose.pose.position.x; 158 | transformSum[4] = laserOdometry->pose.pose.position.y; 159 | transformSum[5] = laserOdometry->pose.pose.position.z; 160 | 161 | transformAssociateToMap(); 162 | 163 | geoQuat = tf::createQuaternionMsgFromRollPitchYaw 164 | (transformMapped[2], -transformMapped[0], -transformMapped[1]); 165 | 166 | laserOdometry2.header.stamp = laserOdometry->header.stamp; 167 | laserOdometry2.pose.pose.orientation.x = -geoQuat.y; 168 | laserOdometry2.pose.pose.orientation.y = -geoQuat.z; 169 | laserOdometry2.pose.pose.orientation.z = geoQuat.x; 170 | laserOdometry2.pose.pose.orientation.w = geoQuat.w; 171 | laserOdometry2.pose.pose.position.x = transformMapped[3]; 172 | laserOdometry2.pose.pose.position.y = transformMapped[4]; 173 | laserOdometry2.pose.pose.position.z = transformMapped[5]; 174 | pubLaserOdometry2Pointer->publish(laserOdometry2); 175 | 176 | laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; 177 | laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 178 | laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5])); 179 | tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2); 180 | } 181 | 182 | void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped) 183 | { 184 | double roll, pitch, yaw; 185 | geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation; 186 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 187 | 188 | transformAftMapped[0] = -pitch; 189 | transformAftMapped[1] = -yaw; 190 | transformAftMapped[2] = roll; 191 | 192 | transformAftMapped[3] = odomAftMapped->pose.pose.position.x; 193 | transformAftMapped[4] = odomAftMapped->pose.pose.position.y; 194 | transformAftMapped[5] = odomAftMapped->pose.pose.position.z; 195 | 196 | transformBefMapped[0] = odomAftMapped->twist.twist.angular.x; 197 | transformBefMapped[1] = odomAftMapped->twist.twist.angular.y; 198 | transformBefMapped[2] = odomAftMapped->twist.twist.angular.z; 199 | 200 | transformBefMapped[3] = odomAftMapped->twist.twist.linear.x; 201 | transformBefMapped[4] = odomAftMapped->twist.twist.linear.y; 202 | transformBefMapped[5] = odomAftMapped->twist.twist.linear.z; 203 | } 204 | 205 | int main(int argc, char** argv) 206 | { 207 | ros::init(argc, argv, "transformMaintenance"); 208 | ros::NodeHandle nh; 209 | 210 | ros::Subscriber subLaserOdometry = nh.subscribe 211 | ("/laser_odom_to_init", 5, laserOdometryHandler); 212 | 213 | ros::Subscriber subOdomAftMapped = nh.subscribe 214 | ("/aft_mapped_to_init", 5, odomAftMappedHandler); 215 | 216 | ros::Publisher pubLaserOdometry2 = nh.advertise ("/integrated_to_init", 5); 217 | pubLaserOdometry2Pointer = &pubLaserOdometry2; 218 | laserOdometry2.header.frame_id = "/camera_init"; 219 | laserOdometry2.child_frame_id = "/camera"; 220 | 221 | tf::TransformBroadcaster tfBroadcaster2; 222 | tfBroadcaster2Pointer = &tfBroadcaster2; 223 | laserOdometryTrans2.frame_id_ = "/camera_init"; 224 | laserOdometryTrans2.child_frame_id_ = "/camera"; 225 | 226 | ros::spin(); 227 | 228 | return 0; 229 | } 230 | -------------------------------------------------------------------------------- /src/transformMaintenance_c.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 2 | // Further contributions copyright (c) 2016, Southwest Research Institute 3 | // All rights reserved. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // 1. Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // 2. Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // 3. Neither the name of the copyright holder nor the names of its 14 | // contributors may be used to endorse or promote products derived from this 15 | // software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // This is an implementation of the algorithm described in the following paper: 30 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 31 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | float transformSum[6] = {0}; 50 | float transformIncre[6] = {0}; 51 | float transformMapped[6] = {0}; 52 | float transformBefMapped[6] = {0}; 53 | float transformAftMapped[6] = {0}; 54 | 55 | ros::Publisher *pubLaserOdometry2Pointer = NULL; 56 | tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL; 57 | nav_msgs::Odometry laserOdometry2; 58 | tf::StampedTransform laserOdometryTrans2; 59 | 60 | void transformAssociateToMap() 61 | { 62 | float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 63 | - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 64 | float y1 = transformBefMapped[4] - transformSum[4]; 65 | float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) 66 | + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 67 | 68 | float x2 = x1; 69 | float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1; 70 | float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1; 71 | 72 | transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2; 73 | transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2; 74 | transformIncre[5] = z2; 75 | 76 | float sbcx = sin(transformSum[0]); 77 | float cbcx = cos(transformSum[0]); 78 | float sbcy = sin(transformSum[1]); 79 | float cbcy = cos(transformSum[1]); 80 | float sbcz = sin(transformSum[2]); 81 | float cbcz = cos(transformSum[2]); 82 | 83 | float sblx = sin(transformBefMapped[0]); 84 | float cblx = cos(transformBefMapped[0]); 85 | float sbly = sin(transformBefMapped[1]); 86 | float cbly = cos(transformBefMapped[1]); 87 | float sblz = sin(transformBefMapped[2]); 88 | float cblz = cos(transformBefMapped[2]); 89 | 90 | float salx = sin(transformAftMapped[0]); 91 | float calx = cos(transformAftMapped[0]); 92 | float saly = sin(transformAftMapped[1]); 93 | float caly = cos(transformAftMapped[1]); 94 | float salz = sin(transformAftMapped[2]); 95 | float calz = cos(transformAftMapped[2]); 96 | 97 | float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 98 | - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 99 | - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 100 | - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 101 | - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); 102 | transformMapped[0] = -asin(srx); 103 | 104 | float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 105 | - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 106 | - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 107 | - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 108 | + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 109 | float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 110 | - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 111 | - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 112 | - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 113 | + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 114 | transformMapped[1] = atan2(srycrx / cos(transformMapped[0]), crycrx / cos(transformMapped[0])); 115 | 116 | float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 117 | - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 118 | - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 119 | + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 120 | - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 121 | + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 122 | + calx*cblx*salz*sblz); 123 | float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 124 | - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 125 | + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 126 | + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 127 | + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 128 | - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 129 | - calx*calz*cblx*sblz); 130 | transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]), crzcrx / cos(transformMapped[0])); 131 | 132 | x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4]; 133 | y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4]; 134 | z1 = transformIncre[5]; 135 | 136 | x2 = x1; 137 | y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1; 138 | z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1; 139 | 140 | transformMapped[3] = transformAftMapped[3] 141 | - (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2); 142 | transformMapped[4] = transformAftMapped[4] - y2; 143 | transformMapped[5] = transformAftMapped[5] 144 | - (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2); 145 | } 146 | 147 | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) 148 | { 149 | 150 | // obtaining global coarse odometry from 'laser_odom_to_init' after laser odometry node. 151 | double roll, pitch, yaw; 152 | geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; 153 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 154 | 155 | transformSum[0] = -pitch; 156 | transformSum[1] = -yaw; 157 | transformSum[2] = roll; 158 | 159 | transformSum[3] = laserOdometry->pose.pose.position.x; 160 | transformSum[4] = laserOdometry->pose.pose.position.y; 161 | transformSum[5] = laserOdometry->pose.pose.position.z; 162 | 163 | transformAssociateToMap(); 164 | 165 | geoQuat = tf::createQuaternionMsgFromRollPitchYaw 166 | (transformMapped[2], -transformMapped[0], -transformMapped[1]); 167 | 168 | laserOdometry2.header.stamp = laserOdometry->header.stamp; 169 | laserOdometry2.pose.pose.orientation.x = -geoQuat.y; 170 | laserOdometry2.pose.pose.orientation.y = -geoQuat.z; 171 | laserOdometry2.pose.pose.orientation.z = geoQuat.x; 172 | laserOdometry2.pose.pose.orientation.w = geoQuat.w; 173 | laserOdometry2.pose.pose.position.x = transformMapped[3]; 174 | laserOdometry2.pose.pose.position.y = transformMapped[4]; 175 | laserOdometry2.pose.pose.position.z = transformMapped[5]; 176 | pubLaserOdometry2Pointer->publish(laserOdometry2); 177 | 178 | laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; 179 | laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 180 | laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5])); 181 | tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2); 182 | } 183 | 184 | // this callback function extract relative transform 'transformBefMapped' and global transform 185 | // 'transformAftMapped' from global refined odometry 'aft_mapped_to_init' ROS messages 186 | void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped) 187 | { 188 | double roll, pitch, yaw; 189 | geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation; 190 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 191 | 192 | transformAftMapped[0] = -pitch; 193 | transformAftMapped[1] = -yaw; 194 | transformAftMapped[2] = roll; 195 | 196 | transformAftMapped[3] = odomAftMapped->pose.pose.position.x; 197 | transformAftMapped[4] = odomAftMapped->pose.pose.position.y; 198 | transformAftMapped[5] = odomAftMapped->pose.pose.position.z; 199 | 200 | transformBefMapped[0] = odomAftMapped->twist.twist.angular.x; 201 | transformBefMapped[1] = odomAftMapped->twist.twist.angular.y; 202 | transformBefMapped[2] = odomAftMapped->twist.twist.angular.z; 203 | 204 | transformBefMapped[3] = odomAftMapped->twist.twist.linear.x; 205 | transformBefMapped[4] = odomAftMapped->twist.twist.linear.y; 206 | transformBefMapped[5] = odomAftMapped->twist.twist.linear.z; 207 | } 208 | 209 | int main(int argc, char** argv) 210 | { 211 | ros::init(argc, argv, "transformMaintenance"); 212 | ros::NodeHandle nh; 213 | 214 | ros::Subscriber subLaserOdometry = nh.subscribe 215 | ("/laser_odom_to_init", 5, laserOdometryHandler); 216 | 217 | ros::Subscriber subOdomAftMapped = nh.subscribe 218 | ("/aft_mapped_to_init", 5, odomAftMappedHandler); 219 | 220 | ros::Publisher pubLaserOdometry2 = nh.advertise ("/integrated_to_init", 5); 221 | pubLaserOdometry2Pointer = &pubLaserOdometry2; 222 | laserOdometry2.header.frame_id = "/camera_init"; 223 | laserOdometry2.child_frame_id = "/camera"; 224 | 225 | tf::TransformBroadcaster tfBroadcaster2; 226 | tfBroadcaster2Pointer = &tfBroadcaster2; 227 | laserOdometryTrans2.frame_id_ = "/camera_init"; 228 | laserOdometryTrans2.child_frame_id_ = "/camera"; 229 | 230 | ros::spin(); 231 | 232 | return 0; 233 | } 234 | --------------------------------------------------------------------------------