├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── capture.bmp ├── include └── loam_velodyne │ ├── Angle.h │ ├── BasicLaserMapping.h │ ├── BasicLaserOdometry.h │ ├── BasicScanRegistration.h │ ├── BasicTransformMaintenance.h │ ├── CircularBuffer.h │ ├── LaserMapping.h │ ├── LaserOdometry.h │ ├── MultiScanRegistration.h │ ├── ScanRegistration.h │ ├── TransformMaintenance.h │ ├── Twist.h │ ├── Vector3.h │ ├── common.h │ ├── nanoflann.hpp │ ├── nanoflann_pcl.h │ └── time_utils.h ├── launch ├── hector_loam_velodyne.launch └── loam_velodyne.launch ├── package.xml ├── rviz_cfg └── loam_velodyne.rviz ├── src ├── laser_mapping_node.cpp ├── laser_odometry_node.cpp ├── lib │ ├── BasicLaserMapping.cpp │ ├── BasicLaserOdometry.cpp │ ├── BasicScanRegistration.cpp │ ├── BasicTransformMaintenance.cpp │ ├── CMakeLists.txt │ ├── LaserMapping.cpp │ ├── LaserOdometry.cpp │ ├── MultiScanRegistration.cpp │ ├── ScanRegistration.cpp │ ├── TransformMaintenance.cpp │ └── math_utils.h ├── multi_scan_registration_node.cpp └── transform_maintenance_node.cpp └── tests ├── bag_test └── loam.test.in /.gitignore: -------------------------------------------------------------------------------- 1 | bin 2 | build 3 | catkin 4 | devel 5 | catkin_generated 6 | gtest 7 | test_results 8 | CMakeFiles 9 | cmake_install.cmake 10 | Makefile 11 | .idea 12 | .vs 13 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1.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 | pcl_conversions) 13 | 14 | find_package(Eigen3 REQUIRED) 15 | find_package(PCL 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 tf pcl_conversions 25 | DEPENDS EIGEN3 PCL 26 | INCLUDE_DIRS include 27 | LIBRARIES loam 28 | ) 29 | 30 | ## Compile as C++14, supported in ROS Kinetic and newer 31 | # set_property(TARGET invz_player PROPERTY CXX_STANDARD 17) 32 | set(CMAKE_CXX_STANDARD 14) 33 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 34 | 35 | add_definitions( -march=native ) 36 | 37 | 38 | add_subdirectory(src/lib) 39 | 40 | add_executable(multiScanRegistration src/multi_scan_registration_node.cpp) 41 | target_link_libraries(multiScanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) 42 | 43 | add_executable(laserOdometry src/laser_odometry_node.cpp) 44 | target_link_libraries(laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) 45 | 46 | add_executable(laserMapping src/laser_mapping_node.cpp) 47 | target_link_libraries(laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) 48 | 49 | add_executable(transformMaintenance src/transform_maintenance_node.cpp) 50 | target_link_libraries(transformMaintenance ${catkin_LIBRARIES} ${PCL_LIBRARIES} loam ) 51 | 52 | if (CATKIN_ENABLE_TESTING) 53 | find_package(rostest REQUIRED) 54 | # TODO: Download test data 55 | catkin_download_test_data(${PROJECT_NAME}_test_data.tar.gz 56 | https://dl.dropboxusercontent.com/s/y4hn486461tfmpm/velodyne_loam_test_data.tar.gz 57 | MD5 3d5194e6981975588b7a93caebf79ba4) 58 | add_custom_target(${PROJECT_NAME}_test_data 59 | COMMAND ${CMAKE_COMMAND} -E tar -xzf velodyne_loam_test_data.tar.gz 60 | DEPENDS ${PROJECT_NAME}_test_data.tar.gz) 61 | configure_file(tests/loam.test.in 62 | ${PROJECT_BINARY_DIR}/test/loam.test) 63 | add_rostest(${PROJECT_BINARY_DIR}/test/loam.test 64 | DEPENDENCIES 65 | ${PROJECT_NAME}_test_data 66 | multiScanRegistration 67 | laserOdometry 68 | laserMapping 69 | transformMaintenance) 70 | endif() 71 | 72 | 73 | -------------------------------------------------------------------------------- /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.md: -------------------------------------------------------------------------------- 1 | # loam_velodyne 2 | 3 | ![Screenshot](/capture.bmp) 4 | Sample map built from [nsh_indoor_outdoor.bag](http://www.frc.ri.cmu.edu/~jizhang03/Datasets/nsh_indoor_outdoor.bag) (opened with [ccViewer](http://www.danielgm.net/cc/)) 5 | 6 | :white_check_mark: Tested with ROS Indigo and Velodyne VLP16. [(Screencast)](https://youtu.be/o1cLXY-Es54) 7 | 8 | All sources were taken from [ROS documentation](http://docs.ros.org/indigo/api/loam_velodyne/html/files.html) 9 | 10 | Ask questions [here](https://github.com/laboshinl/loam_velodyne/issues/3). 11 | 12 | ## How to build with catkin 13 | 14 | ``` 15 | $ cd ~/catkin_ws/src/ 16 | $ git clone https://github.com/laboshinl/loam_velodyne.git 17 | $ cd ~/catkin_ws 18 | $ catkin_make -DCMAKE_BUILD_TYPE=Release 19 | $ source ~/catkin_ws/devel/setup.bash 20 | ``` 21 | 22 | ## Running 23 | 24 | ``` 25 | roslaunch loam_velodyne loam_velodyne.launch 26 | ``` 27 | 28 | In second terminal play sample velodyne data from [VLP16 rosbag](http://www.frc.ri.cmu.edu/~jizhang03/Datasets/): 29 | ``` 30 | rosbag play ~/Downloads/velodyne.bag 31 | ``` 32 | 33 | Or read from velodyne [VLP16 sample pcap](https://midas3.kitware.com/midas/folder/12979): 34 | ``` 35 | roslaunch velodyne_pointcloud VLP16_points.launch pcap:="$HOME/Downloads/velodyne.pcap" 36 | ``` 37 | 38 | ## Troubleshooting 39 | 40 | ### `multiScanRegistration` crashes right after playing bag file 41 | 42 | Issues [#71](https://github.com/laboshinl/loam_velodyne/issues/71) and 43 | [#7](https://github.com/laboshinl/loam_velodyne/issues/7) address this 44 | problem. The current known solution is to build the same version of PCL that 45 | you have on your system from source, and set the `CMAKE_PREFIX_PATH` 46 | accordingly so that catkin can find it. See [this 47 | issue](https://github.com/laboshinl/loam_velodyne/issues/71#issuecomment-416024816) 48 | for more details. 49 | 50 | 51 | --- 52 | [Quantifying Aerial LiDAR Accuracy of LOAM for Civil Engineering Applications.](https://ceen.et.byu.edu/sites/default/files/snrprojects/wolfe_derek.pdf) Derek Anthony Wolfe 53 | 54 | [ROS & Loam_velodyne](https://ishiguro440.wordpress.com/2016/04/05/%E5%82%99%E5%BF%98%E9%8C%B2%E3%80%80ros-loam_velodyne/) 55 | -------------------------------------------------------------------------------- /capture.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/laboshinl/loam_velodyne/25db5dd5b2c135e779a50a11af0a53434598df7e/capture.bmp -------------------------------------------------------------------------------- /include/loam_velodyne/Angle.h: -------------------------------------------------------------------------------- 1 | #ifndef LOAM_ANGLE_H 2 | #define LOAM_ANGLE_H 3 | 4 | #define _USE_MATH_DEFINES 5 | #include 6 | #include 7 | 8 | 9 | namespace loam { 10 | 11 | 12 | /** \brief Class for holding an angle. 13 | * 14 | * This class provides buffered access to sine and cosine values to the represented angular value. 15 | */ 16 | class Angle { 17 | public: 18 | Angle() 19 | : _radian(0.0), 20 | _cos(1.0), 21 | _sin(0.0) {} 22 | 23 | Angle(float radValue) 24 | : _radian(radValue), 25 | _cos(std::cos(radValue)), 26 | _sin(std::sin(radValue)) {} 27 | 28 | Angle(const Angle &other) 29 | : _radian(other._radian), 30 | _cos(other._cos), 31 | _sin(other._sin) {} 32 | 33 | void operator=(const Angle &rhs) { 34 | _radian = (rhs._radian); 35 | _cos = (rhs._cos); 36 | _sin = (rhs._sin); 37 | } 38 | 39 | void operator+=(const float &radValue) { *this = (_radian + radValue); } 40 | 41 | void operator+=(const Angle &other) { *this = (_radian + other._radian); } 42 | 43 | void operator-=(const float &radValue) { *this = (_radian - radValue); } 44 | 45 | void operator-=(const Angle &other) { *this = (_radian - other._radian); } 46 | 47 | Angle operator-() const { 48 | Angle out; 49 | out._radian = -_radian; 50 | out._cos = _cos; 51 | out._sin = -(_sin); 52 | return out; 53 | } 54 | 55 | float rad() const { return _radian; } 56 | 57 | float deg() const { return float(_radian * 180 / M_PI); } 58 | 59 | float cos() const { return _cos; } 60 | 61 | float sin() const { return _sin; } 62 | 63 | private: 64 | float _radian; ///< angle value in radian 65 | float _cos; ///< cosine of the angle 66 | float _sin; ///< sine of the angle 67 | }; 68 | 69 | } // end namespace loam 70 | 71 | #endif //LOAM_ANGLE_H 72 | -------------------------------------------------------------------------------- /include/loam_velodyne/BasicLaserMapping.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 3 | // Further contributions copyright (c) 2016, Southwest Research Institute 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // 2. Redistributions in binary form must reproduce the above copyright notice, 12 | // this list of conditions and the following disclaimer in the documentation 13 | // and/or other materials provided with the distribution. 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // This is an implementation of the algorithm described in the following paper: 31 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 32 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 33 | 34 | 35 | #include "Twist.h" 36 | #include "CircularBuffer.h" 37 | #include "time_utils.h" 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | namespace loam 44 | { 45 | 46 | /** IMU state data. */ 47 | typedef struct IMUState2 48 | { 49 | /** The time of the measurement leading to this state (in seconds). */ 50 | Time stamp; 51 | 52 | /** The current roll angle. */ 53 | Angle roll; 54 | 55 | /** The current pitch angle. */ 56 | Angle pitch; 57 | 58 | /** \brief Interpolate between two IMU states. 59 | * 60 | * @param start the first IMU state 61 | * @param end the second IMU state 62 | * @param ratio the interpolation ratio 63 | * @param result the target IMU state for storing the interpolation result 64 | */ 65 | static void interpolate(const IMUState2& start, 66 | const IMUState2& end, 67 | const float& ratio, 68 | IMUState2& result) 69 | { 70 | float invRatio = 1 - ratio; 71 | 72 | result.roll = start.roll.rad() * invRatio + end.roll.rad() * ratio; 73 | result.pitch = start.pitch.rad() * invRatio + end.pitch.rad() * ratio; 74 | }; 75 | } IMUState2; 76 | 77 | class BasicLaserMapping 78 | { 79 | public: 80 | explicit BasicLaserMapping(const float& scanPeriod = 0.1, const size_t& maxIterations = 10); 81 | 82 | /** \brief Try to process buffered data. */ 83 | bool process(Time const& laserOdometryTime); 84 | void updateIMU(IMUState2 const& newState); 85 | void updateOdometry(double pitch, double yaw, double roll, double x, double y, double z); 86 | void updateOdometry(Twist const& twist); 87 | 88 | auto& laserCloud() { return *_laserCloudFullRes; } 89 | auto& laserCloudCornerLast() { return *_laserCloudCornerLast; } 90 | auto& laserCloudSurfLast() { return *_laserCloudSurfLast; } 91 | 92 | void setScanPeriod(float val) { _scanPeriod = val; } 93 | void setMaxIterations(size_t val) { _maxIterations = val; } 94 | void setDeltaTAbort(float val) { _deltaTAbort = val; } 95 | void setDeltaRAbort(float val) { _deltaRAbort = val; } 96 | 97 | auto& downSizeFilterCorner() { return _downSizeFilterCorner; } 98 | auto& downSizeFilterSurf() { return _downSizeFilterSurf; } 99 | auto& downSizeFilterMap() { return _downSizeFilterMap; } 100 | 101 | auto frameCount() const { return _frameCount; } 102 | auto scanPeriod() const { return _scanPeriod; } 103 | auto maxIterations() const { return _maxIterations; } 104 | auto deltaTAbort() const { return _deltaTAbort; } 105 | auto deltaRAbort() const { return _deltaRAbort; } 106 | 107 | auto const& transformAftMapped() const { return _transformAftMapped; } 108 | auto const& transformBefMapped() const { return _transformBefMapped; } 109 | auto const& laserCloudSurroundDS() const { return *_laserCloudSurroundDS; } 110 | 111 | bool hasFreshMap() const { return _downsizedMapCreated; } 112 | 113 | private: 114 | /** Run an optimization. */ 115 | void optimizeTransformTobeMapped(); 116 | 117 | void transformAssociateToMap(); 118 | void transformUpdate(); 119 | void pointAssociateToMap(const pcl::PointXYZI& pi, pcl::PointXYZI& po); 120 | void pointAssociateTobeMapped(const pcl::PointXYZI& pi, pcl::PointXYZI& po); 121 | void transformFullResToMap(); 122 | 123 | bool createDownsizedMap(); 124 | 125 | // private: 126 | size_t toIndex(int i, int j, int k) const 127 | { return i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k; } 128 | 129 | private: 130 | Time _laserOdometryTime; 131 | 132 | float _scanPeriod; ///< time per scan 133 | const int _stackFrameNum; 134 | const int _mapFrameNum; 135 | long _frameCount; 136 | long _mapFrameCount; 137 | 138 | size_t _maxIterations; ///< maximum number of iterations 139 | float _deltaTAbort; ///< optimization abort threshold for deltaT 140 | float _deltaRAbort; ///< optimization abort threshold for deltaR 141 | 142 | int _laserCloudCenWidth; 143 | int _laserCloudCenHeight; 144 | int _laserCloudCenDepth; 145 | const size_t _laserCloudWidth; 146 | const size_t _laserCloudHeight; 147 | const size_t _laserCloudDepth; 148 | const size_t _laserCloudNum; 149 | 150 | pcl::PointCloud::Ptr _laserCloudCornerLast; ///< last corner points cloud 151 | pcl::PointCloud::Ptr _laserCloudSurfLast; ///< last surface points cloud 152 | pcl::PointCloud::Ptr _laserCloudFullRes; ///< last full resolution cloud 153 | 154 | pcl::PointCloud::Ptr _laserCloudCornerStack; 155 | pcl::PointCloud::Ptr _laserCloudSurfStack; 156 | pcl::PointCloud::Ptr _laserCloudCornerStackDS; ///< down sampled 157 | pcl::PointCloud::Ptr _laserCloudSurfStackDS; ///< down sampled 158 | 159 | pcl::PointCloud::Ptr _laserCloudSurround; 160 | pcl::PointCloud::Ptr _laserCloudSurroundDS; ///< down sampled 161 | pcl::PointCloud::Ptr _laserCloudCornerFromMap; 162 | pcl::PointCloud::Ptr _laserCloudSurfFromMap; 163 | 164 | pcl::PointCloud _laserCloudOri; 165 | pcl::PointCloud _coeffSel; 166 | 167 | std::vector::Ptr> _laserCloudCornerArray; 168 | std::vector::Ptr> _laserCloudSurfArray; 169 | std::vector::Ptr> _laserCloudCornerDSArray; ///< down sampled 170 | std::vector::Ptr> _laserCloudSurfDSArray; ///< down sampled 171 | 172 | std::vector _laserCloudValidInd; 173 | std::vector _laserCloudSurroundInd; 174 | 175 | Twist _transformSum, _transformIncre, _transformTobeMapped, _transformBefMapped, _transformAftMapped; 176 | 177 | CircularBuffer _imuHistory; ///< history of IMU states 178 | 179 | pcl::VoxelGrid _downSizeFilterCorner; ///< voxel filter for down sizing corner clouds 180 | pcl::VoxelGrid _downSizeFilterSurf; ///< voxel filter for down sizing surface clouds 181 | pcl::VoxelGrid _downSizeFilterMap; ///< voxel filter for down sizing accumulated map 182 | 183 | bool _downsizedMapCreated = false; 184 | }; 185 | 186 | } // end namespace loam 187 | 188 | 189 | 190 | 191 | 192 | -------------------------------------------------------------------------------- /include/loam_velodyne/BasicLaserOdometry.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "Twist.h" 3 | #include "nanoflann_pcl.h" 4 | #include 5 | #include 6 | 7 | namespace loam 8 | { 9 | 10 | /** \brief Implementation of the LOAM laser odometry component. 11 | * 12 | */ 13 | class BasicLaserOdometry 14 | { 15 | public: 16 | explicit BasicLaserOdometry(float scanPeriod = 0.1, size_t maxIterations = 25); 17 | 18 | /** \brief Try to process buffered data. */ 19 | void process(); 20 | void updateIMU(pcl::PointCloud const& imuTrans); 21 | 22 | auto& cornerPointsSharp() { return _cornerPointsSharp; } 23 | auto& cornerPointsLessSharp() { return _cornerPointsLessSharp; } 24 | auto& surfPointsFlat() { return _surfPointsFlat; } 25 | auto& surfPointsLessFlat() { return _surfPointsLessFlat; } 26 | auto& laserCloud() { return _laserCloud; } 27 | 28 | auto const& transformSum() { return _transformSum; } 29 | auto const& transform() { return _transform; } 30 | auto const& lastCornerCloud () { return _lastCornerCloud ; } 31 | auto const& lastSurfaceCloud() { return _lastSurfaceCloud; } 32 | 33 | void setScanPeriod(float val) { _scanPeriod = val; } 34 | void setMaxIterations(size_t val) { _maxIterations = val; } 35 | void setDeltaTAbort(float val) { _deltaTAbort = val; } 36 | void setDeltaRAbort(float val) { _deltaRAbort = val; } 37 | 38 | auto frameCount() const { return _frameCount; } 39 | auto scanPeriod() const { return _scanPeriod; } 40 | auto maxIterations() const { return _maxIterations; } 41 | auto deltaTAbort() const { return _deltaTAbort; } 42 | auto deltaRAbort() const { return _deltaRAbort; } 43 | 44 | /** \brief Transform the given point cloud to the end of the sweep. 45 | * 46 | * @param cloud the point cloud to transform 47 | */ 48 | size_t transformToEnd(pcl::PointCloud::Ptr& cloud); 49 | 50 | private: 51 | /** \brief Transform the given point to the start of the sweep. 52 | * 53 | * @param pi the point to transform 54 | * @param po the point instance for storing the result 55 | */ 56 | void transformToStart(const pcl::PointXYZI& pi, pcl::PointXYZI& po); 57 | 58 | 59 | void pluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz, 60 | const Angle& blx, const Angle& bly, const Angle& blz, 61 | const Angle& alx, const Angle& aly, const Angle& alz, 62 | Angle &acx, Angle &acy, Angle &acz); 63 | 64 | void accumulateRotation(Angle cx, Angle cy, Angle cz, 65 | Angle lx, Angle ly, Angle lz, 66 | Angle &ox, Angle &oy, Angle &oz); 67 | 68 | private: 69 | float _scanPeriod; ///< time per scan 70 | long _frameCount; ///< number of processed frames 71 | size_t _maxIterations; ///< maximum number of iterations 72 | bool _systemInited; ///< initialization flag 73 | 74 | float _deltaTAbort; ///< optimization abort threshold for deltaT 75 | float _deltaRAbort; ///< optimization abort threshold for deltaR 76 | 77 | pcl::PointCloud::Ptr _lastCornerCloud; ///< last corner points cloud 78 | pcl::PointCloud::Ptr _lastSurfaceCloud; ///< last surface points cloud 79 | 80 | pcl::PointCloud::Ptr _laserCloudOri; ///< point selection 81 | pcl::PointCloud::Ptr _coeffSel; ///< point selection coefficients 82 | 83 | nanoflann::KdTreeFLANN _lastCornerKDTree; ///< last corner cloud KD-tree 84 | nanoflann::KdTreeFLANN _lastSurfaceKDTree; ///< last surface cloud KD-tree 85 | 86 | pcl::PointCloud::Ptr _cornerPointsSharp; ///< sharp corner points cloud 87 | pcl::PointCloud::Ptr _cornerPointsLessSharp; ///< less sharp corner points cloud 88 | pcl::PointCloud::Ptr _surfPointsFlat; ///< flat surface points cloud 89 | pcl::PointCloud::Ptr _surfPointsLessFlat; ///< less flat surface points cloud 90 | pcl::PointCloud::Ptr _laserCloud; ///< full resolution cloud 91 | 92 | std::vector _pointSearchCornerInd1; ///< first corner point search index buffer 93 | std::vector _pointSearchCornerInd2; ///< second corner point search index buffer 94 | 95 | std::vector _pointSearchSurfInd1; ///< first surface point search index buffer 96 | std::vector _pointSearchSurfInd2; ///< second surface point search index buffer 97 | std::vector _pointSearchSurfInd3; ///< third surface point search index buffer 98 | 99 | Twist _transform; ///< optimized pose transformation 100 | Twist _transformSum; ///< accumulated optimized pose transformation 101 | 102 | Angle _imuRollStart, _imuPitchStart, _imuYawStart; 103 | Angle _imuRollEnd, _imuPitchEnd, _imuYawEnd; 104 | 105 | Vector3 _imuShiftFromStart; 106 | Vector3 _imuVeloFromStart; 107 | }; 108 | 109 | } // end namespace loam 110 | -------------------------------------------------------------------------------- /include/loam_velodyne/BasicScanRegistration.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "Angle.h" 9 | #include "Vector3.h" 10 | #include "CircularBuffer.h" 11 | #include "time_utils.h" 12 | 13 | namespace loam 14 | { 15 | 16 | 17 | 18 | /** \brief A pair describing the start end end index of a range. */ 19 | typedef std::pair IndexRange; 20 | 21 | 22 | 23 | /** Point label options. */ 24 | enum PointLabel 25 | { 26 | CORNER_SHARP = 2, ///< sharp corner point 27 | CORNER_LESS_SHARP = 1, ///< less sharp corner point 28 | SURFACE_LESS_FLAT = 0, ///< less flat surface point 29 | SURFACE_FLAT = -1 ///< flat surface point 30 | }; 31 | 32 | 33 | /** Scan Registration configuration parameters. */ 34 | class RegistrationParams 35 | { 36 | public: 37 | RegistrationParams(const float& scanPeriod_ = 0.1, 38 | const int& imuHistorySize_ = 200, 39 | const int& nFeatureRegions_ = 6, 40 | const int& curvatureRegion_ = 5, 41 | const int& maxCornerSharp_ = 2, 42 | const int& maxSurfaceFlat_ = 4, 43 | const float& lessFlatFilterSize_ = 0.2, 44 | const float& surfaceCurvatureThreshold_ = 0.1); 45 | 46 | /** The time per scan. */ 47 | float scanPeriod; 48 | 49 | /** The size of the IMU history state buffer. */ 50 | int imuHistorySize; 51 | 52 | /** The number of (equally sized) regions used to distribute the feature extraction within a scan. */ 53 | int nFeatureRegions; 54 | 55 | /** The number of surrounding points (+/- region around a point) used to calculate a point curvature. */ 56 | int curvatureRegion; 57 | 58 | /** The maximum number of sharp corner points per feature region. */ 59 | int maxCornerSharp; 60 | 61 | /** The maximum number of less sharp corner points per feature region. */ 62 | int maxCornerLessSharp; 63 | 64 | /** The maximum number of flat surface points per feature region. */ 65 | int maxSurfaceFlat; 66 | 67 | /** The voxel size used for down sizing the remaining less flat surface points. */ 68 | float lessFlatFilterSize; 69 | 70 | /** The curvature threshold below / above a point is considered a flat / corner point. */ 71 | float surfaceCurvatureThreshold; 72 | }; 73 | 74 | 75 | 76 | /** IMU state data. */ 77 | typedef struct IMUState 78 | { 79 | /** The time of the measurement leading to this state (in seconds). */ 80 | Time stamp; 81 | 82 | /** The current roll angle. */ 83 | Angle roll; 84 | 85 | /** The current pitch angle. */ 86 | Angle pitch; 87 | 88 | /** The current yaw angle. */ 89 | Angle yaw; 90 | 91 | /** The accumulated global IMU position in 3D space. */ 92 | Vector3 position; 93 | 94 | /** The accumulated global IMU velocity in 3D space. */ 95 | Vector3 velocity; 96 | 97 | /** The current (local) IMU acceleration in 3D space. */ 98 | Vector3 acceleration; 99 | 100 | /** \brief Interpolate between two IMU states. 101 | * 102 | * @param start the first IMUState 103 | * @param end the second IMUState 104 | * @param ratio the interpolation ratio 105 | * @param result the target IMUState for storing the interpolation result 106 | */ 107 | static void interpolate(const IMUState& start, 108 | const IMUState& end, 109 | const float& ratio, 110 | IMUState& result) 111 | { 112 | float invRatio = 1 - ratio; 113 | 114 | result.roll = start.roll.rad() * invRatio + end.roll.rad() * ratio; 115 | result.pitch = start.pitch.rad() * invRatio + end.pitch.rad() * ratio; 116 | if (start.yaw.rad() - end.yaw.rad() > M_PI) 117 | { 118 | result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() + 2 * M_PI) * ratio; 119 | } 120 | else if (start.yaw.rad() - end.yaw.rad() < -M_PI) 121 | { 122 | result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() - 2 * M_PI) * ratio; 123 | } 124 | else 125 | { 126 | result.yaw = start.yaw.rad() * invRatio + end.yaw.rad() * ratio; 127 | } 128 | 129 | result.velocity = start.velocity * invRatio + end.velocity * ratio; 130 | result.position = start.position * invRatio + end.position * ratio; 131 | }; 132 | } IMUState; 133 | 134 | 135 | class BasicScanRegistration 136 | { 137 | public: 138 | /** \brief Process a new cloud as a set of scanlines. 139 | * 140 | * @param relTime the time relative to the scan time 141 | */ 142 | void processScanlines(const Time& scanTime, std::vector> const& laserCloudScans); 143 | 144 | bool configure(const RegistrationParams& config = RegistrationParams()); 145 | 146 | /** \brief Update new IMU state. NOTE: MUTATES ARGS! */ 147 | void updateIMUData(Vector3& acc, IMUState& newState); 148 | 149 | /** \brief Project a point to the start of the sweep using corresponding IMU data 150 | * 151 | * @param point The point to modify 152 | * @param relTime The time to project by 153 | */ 154 | void projectPointToStartOfSweep(pcl::PointXYZI& point, float relTime); 155 | 156 | auto const& imuTransform () { return _imuTrans ; } 157 | auto const& sweepStart () { return _sweepStart ; } 158 | auto const& laserCloud () { return _laserCloud ; } 159 | auto const& cornerPointsSharp () { return _cornerPointsSharp ; } 160 | auto const& cornerPointsLessSharp () { return _cornerPointsLessSharp; } 161 | auto const& surfacePointsFlat () { return _surfacePointsFlat ; } 162 | auto const& surfacePointsLessFlat () { return _surfacePointsLessFlat; } 163 | auto const& config () { return _config ; } 164 | 165 | private: 166 | 167 | /** \brief Check is IMU data is available. */ 168 | inline bool hasIMUData() { return _imuHistory.size() > 0; }; 169 | 170 | /** \brief Set up the current IMU transformation for the specified relative time. 171 | * 172 | * @param relTime the time relative to the scan time 173 | */ 174 | void setIMUTransformFor(const float& relTime); 175 | 176 | /** \brief Project the given point to the start of the sweep, using the current IMU state and position shift. 177 | * 178 | * @param point the point to project 179 | */ 180 | void transformToStartIMU(pcl::PointXYZI& point); 181 | 182 | /** \brief Prepare for next scan / sweep. 183 | * 184 | * @param scanTime the current scan time 185 | * @param newSweep indicator if a new sweep has started 186 | */ 187 | void reset(const Time& scanTime); 188 | 189 | /** \brief Extract features from current laser cloud. 190 | * 191 | * @param beginIdx the index of the first scan to extract features from 192 | */ 193 | void extractFeatures(const uint16_t& beginIdx = 0); 194 | 195 | /** \brief Set up region buffers for the specified point range. 196 | * 197 | * @param startIdx the region start index 198 | * @param endIdx the region end index 199 | */ 200 | void setRegionBuffersFor(const size_t& startIdx, 201 | const size_t& endIdx); 202 | 203 | /** \brief Set up scan buffers for the specified point range. 204 | * 205 | * @param startIdx the scan start index 206 | * @param endIdx the scan start index 207 | */ 208 | void setScanBuffersFor(const size_t& startIdx, 209 | const size_t& endIdx); 210 | 211 | /** \brief Mark a point and its neighbors as picked. 212 | * 213 | * This method will mark neighboring points within the curvature region as picked, 214 | * as long as they remain within close distance to each other. 215 | * 216 | * @param cloudIdx the index of the picked point in the full resolution cloud 217 | * @param scanIdx the index of the picked point relative to the current scan 218 | */ 219 | void markAsPicked(const size_t& cloudIdx, 220 | const size_t& scanIdx); 221 | 222 | /** \brief Try to interpolate the IMU state for the given time. 223 | * 224 | * @param relTime the time relative to the scan time 225 | * @param outputState the output state instance 226 | */ 227 | void interpolateIMUStateFor(const float& relTime, IMUState& outputState); 228 | 229 | void updateIMUTransform(); 230 | 231 | private: 232 | RegistrationParams _config; ///< registration parameter 233 | 234 | pcl::PointCloud _laserCloud; ///< full resolution input cloud 235 | std::vector _scanIndices; ///< start and end indices of the individual scans withing the full resolution cloud 236 | 237 | pcl::PointCloud _cornerPointsSharp; ///< sharp corner points cloud 238 | pcl::PointCloud _cornerPointsLessSharp; ///< less sharp corner points cloud 239 | pcl::PointCloud _surfacePointsFlat; ///< flat surface points cloud 240 | pcl::PointCloud _surfacePointsLessFlat; ///< less flat surface points cloud 241 | 242 | Time _sweepStart; ///< time stamp of beginning of current sweep 243 | Time _scanTime; ///< time stamp of most recent scan 244 | IMUState _imuStart; ///< the interpolated IMU state corresponding to the start time of the currently processed laser scan 245 | IMUState _imuCur; ///< the interpolated IMU state corresponding to the time of the currently processed laser scan point 246 | Vector3 _imuPositionShift; ///< position shift between accumulated IMU position and interpolated IMU position 247 | size_t _imuIdx = 0; ///< the current index in the IMU history 248 | CircularBuffer _imuHistory; ///< history of IMU states for cloud registration 249 | 250 | pcl::PointCloud _imuTrans = { 4,1 }; ///< IMU transformation information 251 | 252 | std::vector _regionCurvature; ///< point curvature buffer 253 | std::vector _regionLabel; ///< point label buffer 254 | std::vector _regionSortIndices; ///< sorted region indices based on point curvature 255 | std::vector _scanNeighborPicked; ///< flag if neighboring point was already picked 256 | }; 257 | 258 | } 259 | 260 | -------------------------------------------------------------------------------- /include/loam_velodyne/BasicTransformMaintenance.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 3 | // Further contributions copyright (c) 2016, Southwest Research Institute 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // 2. Redistributions in binary form must reproduce the above copyright notice, 12 | // this list of conditions and the following disclaimer in the documentation 13 | // and/or other materials provided with the distribution. 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | // 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // POSSIBILITY OF SUCH DAMAGE. 29 | // 30 | // This is an implementation of the algorithm described in the following paper: 31 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 32 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 33 | 34 | #include "Twist.h" 35 | 36 | namespace loam 37 | { 38 | 39 | /** \brief Implementation of the LOAM transformation maintenance component. 40 | * 41 | */ 42 | class BasicTransformMaintenance 43 | { 44 | public: 45 | void updateOdometry(double pitch, double yaw, double roll, double x, double y, double z); 46 | void updateMappingTransform(Twist const& transformAftMapped, Twist const& transformBefMapped); 47 | void updateMappingTransform(double pitch, double yaw, double roll, 48 | double x, double y, double z, 49 | double twist_rot_x, double twist_rot_y, double twist_rot_z, 50 | double twist_pos_x, double twist_pos_y, double twist_pos_z); 51 | 52 | void transformAssociateToMap(); 53 | 54 | // result accessor 55 | auto const& transformMapped() const { return _transformMapped; } 56 | 57 | private: 58 | float _transformSum[6]{}; 59 | float _transformIncre[6]{}; 60 | float _transformMapped[6]{}; 61 | float _transformBefMapped[6]{}; 62 | float _transformAftMapped[6]{}; 63 | }; 64 | 65 | } // end namespace loam 66 | 67 | -------------------------------------------------------------------------------- /include/loam_velodyne/CircularBuffer.h: -------------------------------------------------------------------------------- 1 | #ifndef LOAM_CIRCULARBUFFER_H 2 | #define LOAM_CIRCULARBUFFER_H 3 | 4 | #include 5 | #include 6 | 7 | 8 | namespace loam { 9 | 10 | 11 | 12 | /** \brief Simple circular buffer implementation for storing data history. 13 | * 14 | * @tparam T The buffer element type. 15 | */ 16 | template 17 | class CircularBuffer { 18 | public: 19 | CircularBuffer(const size_t& capacity = 200) 20 | : _capacity(capacity), 21 | _size(0), 22 | _startIdx(0) 23 | { 24 | _buffer = new T[capacity]; 25 | }; 26 | 27 | ~CircularBuffer() 28 | { 29 | delete[] _buffer; 30 | _buffer = NULL; 31 | } 32 | 33 | /** \brief Retrieve the buffer size. 34 | * 35 | * @return the buffer size 36 | */ 37 | const size_t& size() { 38 | return _size; 39 | } 40 | 41 | /** \brief Retrieve the buffer capacity. 42 | * 43 | * @return the buffer capacity 44 | */ 45 | const size_t& capacity() { 46 | return _capacity; 47 | } 48 | 49 | /** \brief Ensure that this buffer has at least the required capacity. 50 | * 51 | * @param reqCapacity the minimum required capacity 52 | */ 53 | void ensureCapacity(const int& reqCapacity) { 54 | if (reqCapacity > 0 && _capacity < reqCapacity) { 55 | // create new buffer and copy (valid) entries 56 | T* newBuffer = new T[reqCapacity]; 57 | for (size_t i = 0; i < _size; i++) { 58 | newBuffer[i] = (*this)[i]; 59 | } 60 | 61 | // switch buffer pointers and delete old buffer 62 | T* oldBuffer = _buffer; 63 | _buffer = newBuffer; 64 | _startIdx = 0; 65 | 66 | delete[] oldBuffer; 67 | } 68 | } 69 | 70 | /** \brief Check if the buffer is empty. 71 | * 72 | * @return true if the buffer is empty, false otherwise 73 | */ 74 | bool empty() { 75 | return _size == 0; 76 | } 77 | 78 | /** \brief Retrieve the i-th element of the buffer. 79 | * 80 | * 81 | * @param i the buffer index 82 | * @return the element at the i-th position 83 | */ 84 | const T& operator[](const size_t& i) { 85 | return _buffer[(_startIdx + i) % _capacity]; 86 | } 87 | 88 | /** \brief Retrieve the first (oldest) element of the buffer. 89 | * 90 | * @return the first element 91 | */ 92 | const T& first() { 93 | return _buffer[_startIdx]; 94 | } 95 | 96 | /** \brief Retrieve the last (latest) element of the buffer. 97 | * 98 | * @return the last element 99 | */ 100 | const T& last() { 101 | size_t idx = _size == 0 ? 0 : (_startIdx + _size - 1) % _capacity; 102 | return _buffer[idx]; 103 | } 104 | 105 | /** \brief Push a new element to the buffer. 106 | * 107 | * If the buffer reached its capacity, the oldest element is overwritten. 108 | * 109 | * @param element the element to push 110 | */ 111 | void push(const T& element) { 112 | if (_size < _capacity) { 113 | _buffer[_size] = element; 114 | _size++; 115 | } else { 116 | _buffer[_startIdx] = element; 117 | _startIdx = (_startIdx + 1) % _capacity; 118 | } 119 | } 120 | 121 | private: 122 | size_t _capacity; ///< buffer capacity 123 | size_t _size; ///< current buffer size 124 | size_t _startIdx; ///< current start index 125 | T* _buffer; ///< internal element buffer 126 | }; 127 | 128 | } // end namespace loam 129 | 130 | #endif //LOAM_CIRCULARBUFFER_H 131 | -------------------------------------------------------------------------------- /include/loam_velodyne/LaserMapping.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 | #ifndef LOAM_LASERMAPPING_H 34 | #define LOAM_LASERMAPPING_H 35 | 36 | 37 | #include "BasicLaserMapping.h" 38 | #include "common.h" 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | 48 | 49 | namespace loam 50 | { 51 | /** \brief Implementation of the LOAM laser mapping component. 52 | * 53 | */ 54 | class LaserMapping : public BasicLaserMapping 55 | { 56 | public: 57 | explicit LaserMapping(const float& scanPeriod = 0.1, const size_t& maxIterations = 10); 58 | 59 | /** \brief Setup component in active mode. 60 | * 61 | * @param node the ROS node handle 62 | * @param privateNode the private ROS node handle 63 | */ 64 | virtual bool setup(ros::NodeHandle& node, ros::NodeHandle& privateNode); 65 | 66 | /** \brief Handler method for a new last corner cloud. 67 | * 68 | * @param cornerPointsLastMsg the new last corner cloud message 69 | */ 70 | void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLastMsg); 71 | 72 | /** \brief Handler method for a new last surface cloud. 73 | * 74 | * @param surfacePointsLastMsg the new last surface cloud message 75 | */ 76 | void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& surfacePointsLastMsg); 77 | 78 | /** \brief Handler method for a new full resolution cloud. 79 | * 80 | * @param laserCloudFullResMsg the new full resolution cloud message 81 | */ 82 | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg); 83 | 84 | /** \brief Handler method for a new laser odometry. 85 | * 86 | * @param laserOdometry the new laser odometry message 87 | */ 88 | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry); 89 | 90 | /** \brief Handler method for IMU messages. 91 | * 92 | * @param imuIn the new IMU message 93 | */ 94 | void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn); 95 | 96 | /** \brief Process incoming messages in a loop until shutdown (used in active mode). */ 97 | void spin(); 98 | 99 | /** \brief Try to process buffered data. */ 100 | void process(); 101 | 102 | 103 | protected: 104 | /** \brief Reset flags, etc. */ 105 | void reset(); 106 | 107 | /** \brief Check if all required information for a new processing step is available. */ 108 | bool hasNewData(); 109 | 110 | /** \brief Publish the current result via the respective topics. */ 111 | void publishResult(); 112 | 113 | private: 114 | ros::Time _timeLaserCloudCornerLast; ///< time of current last corner cloud 115 | ros::Time _timeLaserCloudSurfLast; ///< time of current last surface cloud 116 | ros::Time _timeLaserCloudFullRes; ///< time of current full resolution cloud 117 | ros::Time _timeLaserOdometry; ///< time of current laser odometry 118 | 119 | bool _newLaserCloudCornerLast; ///< flag if a new last corner cloud has been received 120 | bool _newLaserCloudSurfLast; ///< flag if a new last surface cloud has been received 121 | bool _newLaserCloudFullRes; ///< flag if a new full resolution cloud has been received 122 | bool _newLaserOdometry; ///< flag if a new laser odometry has been received 123 | 124 | 125 | nav_msgs::Odometry _odomAftMapped; ///< mapping odometry message 126 | tf::StampedTransform _aftMappedTrans; ///< mapping odometry transformation 127 | 128 | ros::Publisher _pubLaserCloudSurround; ///< map cloud message publisher 129 | ros::Publisher _pubLaserCloudFullRes; ///< current full resolution cloud message publisher 130 | ros::Publisher _pubOdomAftMapped; ///< mapping odometry publisher 131 | tf::TransformBroadcaster _tfBroadcaster; ///< mapping odometry transform broadcaster 132 | 133 | ros::Subscriber _subLaserCloudCornerLast; ///< last corner cloud message subscriber 134 | ros::Subscriber _subLaserCloudSurfLast; ///< last surface cloud message subscriber 135 | ros::Subscriber _subLaserCloudFullRes; ///< full resolution cloud message subscriber 136 | ros::Subscriber _subLaserOdometry; ///< laser odometry message subscriber 137 | ros::Subscriber _subImu; ///< IMU message subscriber 138 | }; 139 | 140 | } // end namespace loam 141 | 142 | #endif //LOAM_LASERMAPPING_H 143 | -------------------------------------------------------------------------------- /include/loam_velodyne/LaserOdometry.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 | #ifndef LOAM_LASERODOMETRY_H 34 | #define LOAM_LASERODOMETRY_H 35 | 36 | 37 | #include "Twist.h" 38 | #include "nanoflann_pcl.h" 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include "BasicLaserOdometry.h" 49 | 50 | namespace loam 51 | { 52 | 53 | /** \brief Implementation of the LOAM laser odometry component. 54 | * 55 | */ 56 | class LaserOdometry : public BasicLaserOdometry 57 | { 58 | public: 59 | explicit LaserOdometry(float scanPeriod = 0.1, uint16_t ioRatio = 2, size_t maxIterations = 25); 60 | 61 | /** \brief Setup component. 62 | * 63 | * @param node the ROS node handle 64 | * @param privateNode the private ROS node handle 65 | */ 66 | virtual bool setup(ros::NodeHandle& node, 67 | ros::NodeHandle& privateNode); 68 | 69 | /** \brief Handler method for a new sharp corner cloud. 70 | * 71 | * @param cornerPointsSharpMsg the new sharp corner cloud message 72 | */ 73 | void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharpMsg); 74 | 75 | /** \brief Handler method for a new less sharp corner cloud. 76 | * 77 | * @param cornerPointsLessSharpMsg the new less sharp corner cloud message 78 | */ 79 | void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharpMsg); 80 | 81 | /** \brief Handler method for a new flat surface cloud. 82 | * 83 | * @param surfPointsFlatMsg the new flat surface cloud message 84 | */ 85 | void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlatMsg); 86 | 87 | /** \brief Handler method for a new less flat surface cloud. 88 | * 89 | * @param surfPointsLessFlatMsg the new less flat surface cloud message 90 | */ 91 | void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlatMsg); 92 | 93 | /** \brief Handler method for a new full resolution cloud. 94 | * 95 | * @param laserCloudFullResMsg the new full resolution cloud message 96 | */ 97 | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg); 98 | 99 | /** \brief Handler method for a new IMU transformation information. 100 | * 101 | * @param laserCloudFullResMsg the new IMU transformation information message 102 | */ 103 | void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTransMsg); 104 | 105 | 106 | /** \brief Process incoming messages in a loop until shutdown (used in active mode). */ 107 | void spin(); 108 | 109 | /** \brief Try to process buffered data. */ 110 | void process(); 111 | 112 | protected: 113 | /** \brief Reset flags, etc. */ 114 | void reset(); 115 | 116 | /** \brief Check if all required information for a new processing step is available. */ 117 | bool hasNewData(); 118 | 119 | /** \brief Publish the current result via the respective topics. */ 120 | void publishResult(); 121 | 122 | private: 123 | uint16_t _ioRatio; ///< ratio of input to output frames 124 | 125 | ros::Time _timeCornerPointsSharp; ///< time of current sharp corner cloud 126 | ros::Time _timeCornerPointsLessSharp; ///< time of current less sharp corner cloud 127 | ros::Time _timeSurfPointsFlat; ///< time of current flat surface cloud 128 | ros::Time _timeSurfPointsLessFlat; ///< time of current less flat surface cloud 129 | ros::Time _timeLaserCloudFullRes; ///< time of current full resolution cloud 130 | ros::Time _timeImuTrans; ///< time of current IMU transformation information 131 | 132 | bool _newCornerPointsSharp; ///< flag if a new sharp corner cloud has been received 133 | bool _newCornerPointsLessSharp; ///< flag if a new less sharp corner cloud has been received 134 | bool _newSurfPointsFlat; ///< flag if a new flat surface cloud has been received 135 | bool _newSurfPointsLessFlat; ///< flag if a new less flat surface cloud has been received 136 | bool _newLaserCloudFullRes; ///< flag if a new full resolution cloud has been received 137 | bool _newImuTrans; ///< flag if a new IMU transformation information cloud has been received 138 | 139 | nav_msgs::Odometry _laserOdometryMsg; ///< laser odometry message 140 | tf::StampedTransform _laserOdometryTrans; ///< laser odometry transformation 141 | 142 | ros::Publisher _pubLaserCloudCornerLast; ///< last corner cloud message publisher 143 | ros::Publisher _pubLaserCloudSurfLast; ///< last surface cloud message publisher 144 | ros::Publisher _pubLaserCloudFullRes; ///< full resolution cloud message publisher 145 | ros::Publisher _pubLaserOdometry; ///< laser odometry publisher 146 | tf::TransformBroadcaster _tfBroadcaster; ///< laser odometry transform broadcaster 147 | 148 | ros::Subscriber _subCornerPointsSharp; ///< sharp corner cloud message subscriber 149 | ros::Subscriber _subCornerPointsLessSharp; ///< less sharp corner cloud message subscriber 150 | ros::Subscriber _subSurfPointsFlat; ///< flat surface cloud message subscriber 151 | ros::Subscriber _subSurfPointsLessFlat; ///< less flat surface cloud message subscriber 152 | ros::Subscriber _subLaserCloudFullRes; ///< full resolution cloud message subscriber 153 | ros::Subscriber _subImuTrans; ///< IMU transformation information message subscriber 154 | }; 155 | 156 | } // end namespace loam 157 | 158 | #endif //LOAM_LASERODOMETRY_H 159 | -------------------------------------------------------------------------------- /include/loam_velodyne/MultiScanRegistration.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 | #ifndef LOAM_MULTISCANREGISTRATION_H 34 | #define LOAM_MULTISCANREGISTRATION_H 35 | 36 | 37 | #include "loam_velodyne/ScanRegistration.h" 38 | 39 | #include 40 | 41 | 42 | namespace loam { 43 | 44 | 45 | 46 | /** \brief Class realizing a linear mapping from vertical point angle to the corresponding scan ring. 47 | * 48 | */ 49 | class MultiScanMapper { 50 | public: 51 | /** \brief Construct a new multi scan mapper instance. 52 | * 53 | * @param lowerBound - the lower vertical bound (degrees) 54 | * @param upperBound - the upper vertical bound (degrees) 55 | * @param nScanRings - the number of scan rings 56 | */ 57 | MultiScanMapper(const float& lowerBound = -15, 58 | const float& upperBound = 15, 59 | const uint16_t& nScanRings = 16); 60 | 61 | const float& getLowerBound() { return _lowerBound; } 62 | const float& getUpperBound() { return _upperBound; } 63 | const uint16_t& getNumberOfScanRings() { return _nScanRings; } 64 | 65 | /** \brief Set mapping parameters. 66 | * 67 | * @param lowerBound - the lower vertical bound (degrees) 68 | * @param upperBound - the upper vertical bound (degrees) 69 | * @param nScanRings - the number of scan rings 70 | */ 71 | void set(const float& lowerBound, 72 | const float& upperBound, 73 | const uint16_t& nScanRings); 74 | 75 | /** \brief Map the specified vertical point angle to its ring ID. 76 | * 77 | * @param angle the vertical point angle (in rad) 78 | * @return the ring ID 79 | */ 80 | int getRingForAngle(const float& angle); 81 | 82 | /** Multi scan mapper for Velodyne VLP-16 according to data sheet. */ 83 | static inline MultiScanMapper Velodyne_VLP_16() { return MultiScanMapper(-15, 15, 16); }; 84 | 85 | /** Multi scan mapper for Velodyne HDL-32 according to data sheet. */ 86 | static inline MultiScanMapper Velodyne_HDL_32() { return MultiScanMapper(-30.67f, 10.67f, 32); }; 87 | 88 | /** Multi scan mapper for Velodyne HDL-64E according to data sheet. */ 89 | static inline MultiScanMapper Velodyne_HDL_64E() { return MultiScanMapper(-24.9f, 2, 64); }; 90 | 91 | 92 | private: 93 | float _lowerBound; ///< the vertical angle of the first scan ring 94 | float _upperBound; ///< the vertical angle of the last scan ring 95 | uint16_t _nScanRings; ///< number of scan rings 96 | float _factor; ///< linear interpolation factor 97 | }; 98 | 99 | 100 | 101 | /** \brief Class for registering point clouds received from multi-laser lidars. 102 | * 103 | */ 104 | class MultiScanRegistration : virtual public ScanRegistration { 105 | public: 106 | MultiScanRegistration(const MultiScanMapper& scanMapper = MultiScanMapper()); 107 | 108 | 109 | bool setup(ros::NodeHandle& node, ros::NodeHandle& privateNode); 110 | 111 | /** \brief Handler method for input cloud messages. 112 | * 113 | * @param laserCloudMsg the new input cloud message to process 114 | */ 115 | void handleCloudMessage(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg); 116 | 117 | private: 118 | /** \brief Setup component in active mode. 119 | * 120 | * @param node the ROS node handle 121 | * @param privateNode the private ROS node handle 122 | */ 123 | bool setupROS(ros::NodeHandle& node, ros::NodeHandle& privateNode, RegistrationParams& config_out) override; 124 | 125 | /** \brief Process a new input cloud. 126 | * 127 | * @param laserCloudIn the new input cloud to process 128 | * @param scanTime the scan (message) timestamp 129 | */ 130 | void process(const pcl::PointCloud& laserCloudIn, const Time& scanTime); 131 | 132 | private: 133 | int _systemDelay = 20; ///< system startup delay counter 134 | MultiScanMapper _scanMapper; ///< mapper for mapping vertical point angles to scan ring IDs 135 | std::vector > _laserCloudScans; 136 | ros::Subscriber _subLaserCloud; ///< input cloud message subscriber 137 | 138 | }; 139 | 140 | } // end namespace loam 141 | 142 | 143 | #endif //LOAM_MULTISCANREGISTRATION_H 144 | -------------------------------------------------------------------------------- /include/loam_velodyne/ScanRegistration.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 | #ifndef LOAM_SCANREGISTRATION_H 34 | #define LOAM_SCANREGISTRATION_H 35 | 36 | 37 | #include "common.h" 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | #include "BasicScanRegistration.h" 45 | 46 | 47 | namespace loam 48 | { 49 | /** \brief Base class for LOAM scan registration implementations. 50 | * 51 | * As there exist various sensor devices, producing differently formatted point clouds, 52 | * specific implementations are needed for each group of sensor devices to achieve an accurate registration. 53 | * This class provides common configurations, buffering and processing logic. 54 | */ 55 | class ScanRegistration : protected BasicScanRegistration 56 | { 57 | public: 58 | 59 | /** \brief Setup component. 60 | * 61 | * @param node the ROS node handle 62 | * @param privateNode the private ROS node handle 63 | */ 64 | virtual bool setupROS(ros::NodeHandle& node, ros::NodeHandle& privateNode, RegistrationParams& config_out); 65 | 66 | /** \brief Handler method for IMU messages. 67 | * 68 | * @param imuIn the new IMU message 69 | */ 70 | virtual void handleIMUMessage(const sensor_msgs::Imu::ConstPtr& imuIn); 71 | 72 | protected: 73 | /** \brief Publish the current result via the respective topics. */ 74 | void publishResult(); 75 | 76 | private: 77 | 78 | /** \brief Parse node parameter. 79 | * 80 | * @param nh the ROS node handle 81 | * @return true, if all specified parameters are valid, false if at least one specified parameter is invalid 82 | */ 83 | bool parseParams(const ros::NodeHandle& nh, RegistrationParams& config_out); 84 | 85 | private: 86 | ros::Subscriber _subImu; ///< IMU message subscriber 87 | ros::Publisher _pubLaserCloud; ///< full resolution cloud message publisher 88 | ros::Publisher _pubCornerPointsSharp; ///< sharp corner cloud message publisher 89 | ros::Publisher _pubCornerPointsLessSharp; ///< less sharp corner cloud message publisher 90 | ros::Publisher _pubSurfPointsFlat; ///< flat surface cloud message publisher 91 | ros::Publisher _pubSurfPointsLessFlat; ///< less flat surface cloud message publisher 92 | ros::Publisher _pubImuTrans; ///< IMU transformation message publisher 93 | }; 94 | 95 | } // end namespace loam 96 | 97 | 98 | #endif //LOAM_SCANREGISTRATION_H 99 | -------------------------------------------------------------------------------- /include/loam_velodyne/TransformMaintenance.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 | #ifndef LOAM_TRANSFORMMAINTENANCE_H 34 | #define LOAM_TRANSFORMMAINTENANCE_H 35 | 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include "loam_velodyne/BasicTransformMaintenance.h" 42 | 43 | namespace loam { 44 | 45 | /** \brief Implementation of the LOAM transformation maintenance component. 46 | * 47 | */ 48 | class TransformMaintenance: public BasicTransformMaintenance { 49 | public: 50 | TransformMaintenance(); 51 | 52 | /** \brief Setup component. 53 | * 54 | * @param node the ROS node handle 55 | * @param privateNode the private ROS node handle 56 | */ 57 | virtual bool setup(ros::NodeHandle& node, ros::NodeHandle& privateNode); 58 | 59 | /** \brief Handler method for laser odometry messages. 60 | * 61 | * @param laserOdometry the new laser odometry 62 | */ 63 | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry); 64 | 65 | /** \brief Handler method for mapping odometry messages. 66 | * 67 | * @param odomAftMapped the new mapping odometry 68 | */ 69 | void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped); 70 | 71 | private: 72 | nav_msgs::Odometry _laserOdometry2; ///< latest integrated laser odometry message 73 | tf::StampedTransform _laserOdometryTrans2; ///< latest integrated laser odometry transformation 74 | 75 | ros::Publisher _pubLaserOdometry2; ///< integrated laser odometry publisher 76 | tf::TransformBroadcaster _tfBroadcaster2; ///< integrated laser odometry transformation broadcaster 77 | 78 | ros::Subscriber _subLaserOdometry; ///< (high frequency) laser odometry subscriber 79 | ros::Subscriber _subOdomAftMapped; ///< (low frequency) mapping odometry subscriber 80 | }; 81 | 82 | } // end namespace loam 83 | 84 | 85 | #endif //LOAM_TRANSFORMMAINTENANCE_H 86 | -------------------------------------------------------------------------------- /include/loam_velodyne/Twist.h: -------------------------------------------------------------------------------- 1 | #ifndef LOAM_TWIST_H 2 | #define LOAM_TWIST_H 3 | 4 | 5 | #include "Angle.h" 6 | #include "Vector3.h" 7 | 8 | 9 | namespace loam { 10 | 11 | 12 | /** \brief Twist composed by three angles and a three-dimensional position. 13 | * 14 | */ 15 | class Twist { 16 | public: 17 | Twist() 18 | : rot_x(), 19 | rot_y(), 20 | rot_z(), 21 | pos() {}; 22 | 23 | Angle rot_x; 24 | Angle rot_y; 25 | Angle rot_z; 26 | Vector3 pos; 27 | }; 28 | 29 | } // end namespace loam 30 | 31 | #endif //LOAM_TWIST_H 32 | -------------------------------------------------------------------------------- /include/loam_velodyne/Vector3.h: -------------------------------------------------------------------------------- 1 | #ifndef LOAM_VECTOR3_H 2 | #define LOAM_VECTOR3_H 3 | 4 | 5 | #include 6 | 7 | 8 | namespace loam { 9 | 10 | /** \brief Vector4f decorator for convenient handling. 11 | * 12 | */ 13 | class Vector3 : public Eigen::Vector4f { 14 | public: 15 | Vector3(float x, float y, float z) 16 | : Eigen::Vector4f(x, y, z, 0) {} 17 | 18 | Vector3(void) 19 | : Eigen::Vector4f(0, 0, 0, 0) {} 20 | 21 | template 22 | Vector3(const Eigen::MatrixBase &other) 23 | : Eigen::Vector4f(other) {} 24 | 25 | Vector3(const pcl::PointXYZI &p) 26 | : Eigen::Vector4f(p.x, p.y, p.z, 0) {} 27 | 28 | template 29 | Vector3 &operator=(const Eigen::MatrixBase &rhs) { 30 | this->Eigen::Vector4f::operator=(rhs); 31 | return *this; 32 | } 33 | 34 | Vector3 &operator=(const pcl::PointXYZ &rhs) { 35 | x() = rhs.x; 36 | y() = rhs.y; 37 | z() = rhs.z; 38 | return *this; 39 | } 40 | 41 | Vector3 &operator=(const pcl::PointXYZI &rhs) { 42 | x() = rhs.x; 43 | y() = rhs.y; 44 | z() = rhs.z; 45 | return *this; 46 | } 47 | 48 | float x() const { return (*this)(0); } 49 | 50 | float y() const { return (*this)(1); } 51 | 52 | float z() const { return (*this)(2); } 53 | 54 | float &x() { return (*this)(0); } 55 | 56 | float &y() { return (*this)(1); } 57 | 58 | float &z() { return (*this)(2); } 59 | 60 | // easy conversion 61 | operator pcl::PointXYZI() { 62 | pcl::PointXYZI dst; 63 | dst.x = x(); 64 | dst.y = y(); 65 | dst.z = z(); 66 | dst.intensity = 0; 67 | return dst; 68 | } 69 | }; 70 | 71 | } // end namespace loam 72 | 73 | #endif //LOAM_VECTOR3_H 74 | -------------------------------------------------------------------------------- /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 | #ifndef LOAM_COMMON_H 34 | #define LOAM_COMMON_H 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include "time_utils.h" 41 | 42 | namespace loam { 43 | 44 | /** \brief Construct a new point cloud message from the specified information and publish it via the given publisher. 45 | * 46 | * @tparam PointT the point type 47 | * @param publisher the publisher instance 48 | * @param cloud the cloud to publish 49 | * @param stamp the time stamp of the cloud message 50 | * @param frameID the message frame ID 51 | */ 52 | template 53 | inline void publishCloudMsg(ros::Publisher& publisher, 54 | const pcl::PointCloud& cloud, 55 | const ros::Time& stamp, 56 | std::string frameID) { 57 | sensor_msgs::PointCloud2 msg; 58 | pcl::toROSMsg(cloud, msg); 59 | msg.header.stamp = stamp; 60 | msg.header.frame_id = frameID; 61 | publisher.publish(msg); 62 | } 63 | 64 | 65 | // ROS time adapters 66 | inline Time fromROSTime(ros::Time const& rosTime) 67 | { 68 | auto epoch = std::chrono::system_clock::time_point(); 69 | auto since_epoch = std::chrono::seconds(rosTime.sec) + std::chrono::nanoseconds(rosTime.nsec); 70 | return epoch + since_epoch; 71 | } 72 | 73 | inline ros::Time toROSTime(Time const& time_point) 74 | { 75 | return ros::Time().fromNSec(std::chrono::duration_cast(time_point.time_since_epoch()).count()); 76 | } 77 | 78 | } // end namespace loam 79 | 80 | #endif // LOAM_COMMON_H 81 | -------------------------------------------------------------------------------- /include/loam_velodyne/nanoflann_pcl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2010-2012, Willow Garage, Inc. 6 | * Copyright (c) 2012-, Open Perception, Inc. 7 | * 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder(s) nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | * 37 | * $Id: kdtree_flann.h 36261 2011-02-26 01:34:42Z mariusm $ 38 | * 39 | */ 40 | 41 | #ifndef NANO_KDTREE_KDTREE_FLANN_H_ 42 | #define NANO_KDTREE_KDTREE_FLANN_H_ 43 | 44 | #include 45 | #include 46 | #include 47 | #include "nanoflann.hpp" 48 | 49 | namespace nanoflann 50 | { 51 | 52 | // Adapter class to give to nanoflann the same "look and fell" of pcl::KdTreeFLANN. 53 | // limited to squared distance between 3D points 54 | template 55 | class KdTreeFLANN 56 | { 57 | public: 58 | 59 | typedef boost::shared_ptr > Ptr; 60 | typedef boost::shared_ptr > ConstPtr; 61 | 62 | typedef typename pcl::PointCloud PointCloud; 63 | typedef typename pcl::PointCloud::Ptr PointCloudPtr; 64 | typedef typename pcl::PointCloud::ConstPtr PointCloudConstPtr; 65 | 66 | typedef boost::shared_ptr > IndicesPtr; 67 | typedef boost::shared_ptr > IndicesConstPtr; 68 | 69 | KdTreeFLANN (bool sorted = true); 70 | 71 | KdTreeFLANN (const KdTreeFLANN &k); 72 | 73 | void setEpsilon (float eps); 74 | 75 | void setSortedResults (bool sorted); 76 | 77 | inline Ptr makeShared () { return Ptr (new KdTreeFLANN (*this)); } 78 | 79 | void setInputCloud (const PointCloudPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()); 80 | 81 | int nearestKSearch (const PointT &point, int k, std::vector &k_indices, 82 | std::vector &k_sqr_distances) const; 83 | 84 | int radiusSearch (const PointT &point, double radius, std::vector &k_indices, 85 | std::vector &k_sqr_distances) const; 86 | 87 | private: 88 | 89 | nanoflann::SearchParams _params; 90 | 91 | struct PointCloud_Adaptor 92 | { 93 | inline size_t kdtree_get_point_count() const; 94 | inline float kdtree_get_pt(const size_t idx, int dim) const; 95 | template bool kdtree_get_bbox(BBOX&) const { return false; } 96 | PointCloudConstPtr pcl; 97 | IndicesConstPtr indices; 98 | }; 99 | 100 | typedef nanoflann::KDTreeSingleIndexAdaptor< 101 | nanoflann::SO3_Adaptor , 102 | PointCloud_Adaptor, 3, int> KDTreeFlann_PCL_SO3; 103 | 104 | PointCloud_Adaptor _adaptor; 105 | 106 | KDTreeFlann_PCL_SO3 _kdtree; 107 | 108 | }; 109 | 110 | //---------- Definitions --------------------- 111 | 112 | template inline 113 | KdTreeFLANN::KdTreeFLANN(bool sorted): 114 | _kdtree(3,_adaptor) 115 | { 116 | _params.sorted = sorted; 117 | } 118 | 119 | template inline 120 | void KdTreeFLANN::setEpsilon(float eps) 121 | { 122 | _params.eps = eps; 123 | } 124 | 125 | template inline 126 | void KdTreeFLANN::setSortedResults(bool sorted) 127 | { 128 | _params.sorted = sorted; 129 | } 130 | 131 | template inline 132 | void KdTreeFLANN::setInputCloud(const KdTreeFLANN::PointCloudPtr &cloud, 133 | const IndicesConstPtr &indices) 134 | { 135 | _adaptor.pcl = cloud; 136 | _adaptor.indices = indices; 137 | _kdtree.buildIndex(); 138 | } 139 | 140 | template inline 141 | int KdTreeFLANN::nearestKSearch(const PointT &point, int num_closest, 142 | std::vector &k_indices, 143 | std::vector &k_sqr_distances) const 144 | { 145 | k_indices.resize(num_closest); 146 | k_sqr_distances.resize(num_closest); 147 | 148 | nanoflann::KNNResultSet resultSet(num_closest); 149 | resultSet.init( k_indices.data(), k_sqr_distances.data()); 150 | _kdtree.findNeighbors(resultSet, point.data, nanoflann::SearchParams() ); 151 | return resultSet.size(); 152 | } 153 | 154 | template inline 155 | int KdTreeFLANN::radiusSearch(const PointT &point, double radius, 156 | std::vector &k_indices, 157 | std::vector &k_sqr_distances) const 158 | { 159 | static std::vector > indices_dist; 160 | indices_dist.reserve( 128 ); 161 | 162 | RadiusResultSet resultSet(radius, indices_dist); 163 | const size_t nFound = _kdtree.findNeighbors(resultSet, point.data, _params); 164 | 165 | if (_params.sorted) 166 | std::sort(indices_dist.begin(), indices_dist.end(), IndexDist_Sorter() ); 167 | 168 | k_indices.resize(nFound); 169 | k_sqr_distances.resize(nFound); 170 | for(int i=0; i inline 178 | size_t KdTreeFLANN::PointCloud_Adaptor::kdtree_get_point_count() const { 179 | if( indices ) return indices->size(); 180 | if( pcl) return pcl->points.size(); 181 | return 0; 182 | } 183 | 184 | template inline 185 | float KdTreeFLANN::PointCloud_Adaptor::kdtree_get_pt(const size_t idx, int dim) const{ 186 | const PointT& p = ( indices ) ? pcl->points[(*indices)[idx]] : pcl->points[idx]; 187 | if (dim==0) return p.x; 188 | else if (dim==1) return p.y; 189 | else if (dim==2) return p.z; 190 | else return 0.0; 191 | } 192 | 193 | } 194 | 195 | 196 | #endif 197 | -------------------------------------------------------------------------------- /include/loam_velodyne/time_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace loam 6 | { 7 | /** \brief A standard non-ROS alternative to ros::Time.*/ 8 | using Time = std::chrono::system_clock::time_point; 9 | 10 | // helper function 11 | inline double toSec(Time::duration duration) 12 | { 13 | return std::chrono::duration(duration).count(); 14 | }; 15 | } 16 | -------------------------------------------------------------------------------- /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 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /launch/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 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /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 | pcl_conversions 28 | 29 | geometry_msgs 30 | nav_msgs 31 | sensor_msgs 32 | roscpp 33 | rospy 34 | std_msgs 35 | tf 36 | pcl_conversions 37 | 38 | rostest 39 | rosbag 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /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: 714 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: 10 97 | Min Value: -10 98 | Value: true 99 | Axis: Z 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: 162.362 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: 0 193 | Y: 0 194 | Z: 0 195 | Name: Current View 196 | Near Clip Distance: 0.01 197 | Pitch: -1.5698 198 | Target Frame: aft_mapped 199 | Value: ThirdPersonFollower (rviz) 200 | Yaw: 4.29039 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: 000000ff00000000fd00000004000000000000013c0000035bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000430000035b000000df00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000430000035b000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077a0000003efc0100000002fb0000000800540069006d006501000000000000077a0000022a00fffffffb0000000800540069006d00650100000000000004500000000000000000000005230000035b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 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: 0 219 | Y: 0 220 | -------------------------------------------------------------------------------- /src/laser_mapping_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "loam_velodyne/LaserMapping.h" 3 | 4 | 5 | /** Main node entry point. */ 6 | int main(int argc, char **argv) 7 | { 8 | ros::init(argc, argv, "laserMapping"); 9 | ros::NodeHandle node; 10 | ros::NodeHandle privateNode("~"); 11 | 12 | loam::LaserMapping laserMapping(0.1); 13 | 14 | if (laserMapping.setup(node, privateNode)) { 15 | // initialization successful 16 | laserMapping.spin(); 17 | } 18 | 19 | return 0; 20 | } 21 | -------------------------------------------------------------------------------- /src/laser_odometry_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "loam_velodyne/LaserOdometry.h" 3 | 4 | 5 | /** Main node entry point. */ 6 | int main(int argc, char **argv) 7 | { 8 | ros::init(argc, argv, "laserOdometry"); 9 | ros::NodeHandle node; 10 | ros::NodeHandle privateNode("~"); 11 | 12 | loam::LaserOdometry laserOdom(0.1); 13 | 14 | if (laserOdom.setup(node, privateNode)) { 15 | // initialization successful 16 | laserOdom.spin(); 17 | } 18 | 19 | return 0; 20 | } 21 | -------------------------------------------------------------------------------- /src/lib/BasicLaserOdometry.cpp: -------------------------------------------------------------------------------- 1 | #include "loam_velodyne/BasicLaserOdometry.h" 2 | 3 | #include "math_utils.h" 4 | #include 5 | #include 6 | #include 7 | 8 | namespace loam 9 | { 10 | 11 | using std::sin; 12 | using std::cos; 13 | using std::asin; 14 | using std::atan2; 15 | using std::sqrt; 16 | using std::fabs; 17 | using std::pow; 18 | 19 | 20 | BasicLaserOdometry::BasicLaserOdometry(float scanPeriod, size_t maxIterations) : 21 | _scanPeriod(scanPeriod), 22 | _systemInited(false), 23 | _frameCount(0), 24 | _maxIterations(maxIterations), 25 | _deltaTAbort(0.1), 26 | _deltaRAbort(0.1), 27 | _cornerPointsSharp(new pcl::PointCloud()), 28 | _cornerPointsLessSharp(new pcl::PointCloud()), 29 | _surfPointsFlat(new pcl::PointCloud()), 30 | _surfPointsLessFlat(new pcl::PointCloud()), 31 | _laserCloud(new pcl::PointCloud()), 32 | _lastCornerCloud(new pcl::PointCloud()), 33 | _lastSurfaceCloud(new pcl::PointCloud()), 34 | _laserCloudOri(new pcl::PointCloud()), 35 | _coeffSel(new pcl::PointCloud()) 36 | {} 37 | 38 | 39 | 40 | void BasicLaserOdometry::transformToStart(const pcl::PointXYZI& pi, pcl::PointXYZI& po) 41 | { 42 | float s = (1.f / _scanPeriod) * (pi.intensity - int(pi.intensity)); 43 | 44 | po.x = pi.x - s * _transform.pos.x(); 45 | po.y = pi.y - s * _transform.pos.y(); 46 | po.z = pi.z - s * _transform.pos.z(); 47 | po.intensity = pi.intensity; 48 | 49 | Angle rx = -s * _transform.rot_x.rad(); 50 | Angle ry = -s * _transform.rot_y.rad(); 51 | Angle rz = -s * _transform.rot_z.rad(); 52 | rotateZXY(po, rz, rx, ry); 53 | } 54 | 55 | 56 | 57 | size_t BasicLaserOdometry::transformToEnd(pcl::PointCloud::Ptr& cloud) 58 | { 59 | size_t cloudSize = cloud->points.size(); 60 | 61 | for (size_t i = 0; i < cloudSize; i++) 62 | { 63 | pcl::PointXYZI& point = cloud->points[i]; 64 | 65 | float s = (1.f / _scanPeriod) * (point.intensity - int(point.intensity)); 66 | 67 | point.x -= s * _transform.pos.x(); 68 | point.y -= s * _transform.pos.y(); 69 | point.z -= s * _transform.pos.z(); 70 | point.intensity = int(point.intensity); 71 | 72 | Angle rx = -s * _transform.rot_x.rad(); 73 | Angle ry = -s * _transform.rot_y.rad(); 74 | Angle rz = -s * _transform.rot_z.rad(); 75 | rotateZXY(point, rz, rx, ry); 76 | rotateYXZ(point, _transform.rot_y, _transform.rot_x, _transform.rot_z); 77 | 78 | point.x += _transform.pos.x() - _imuShiftFromStart.x(); 79 | point.y += _transform.pos.y() - _imuShiftFromStart.y(); 80 | point.z += _transform.pos.z() - _imuShiftFromStart.z(); 81 | 82 | rotateZXY(point, _imuRollStart, _imuPitchStart, _imuYawStart); 83 | rotateYXZ(point, -_imuYawEnd, -_imuPitchEnd, -_imuRollEnd); 84 | } 85 | 86 | return cloudSize; 87 | } 88 | 89 | 90 | 91 | void BasicLaserOdometry::pluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz, 92 | const Angle& blx, const Angle& bly, const Angle& blz, 93 | const Angle& alx, const Angle& aly, const Angle& alz, 94 | Angle &acx, Angle &acy, Angle &acz) 95 | { 96 | float sbcx = bcx.sin(); 97 | float cbcx = bcx.cos(); 98 | float sbcy = bcy.sin(); 99 | float cbcy = bcy.cos(); 100 | float sbcz = bcz.sin(); 101 | float cbcz = bcz.cos(); 102 | 103 | float sblx = blx.sin(); 104 | float cblx = blx.cos(); 105 | float sbly = bly.sin(); 106 | float cbly = bly.cos(); 107 | float sblz = blz.sin(); 108 | float cblz = blz.cos(); 109 | 110 | float salx = alx.sin(); 111 | float calx = alx.cos(); 112 | float saly = aly.sin(); 113 | float caly = aly.cos(); 114 | float salz = alz.sin(); 115 | float calz = alz.cos(); 116 | 117 | float srx = -sbcx * (salx*sblx + calx * caly*cblx*cbly + calx * cblx*saly*sbly) 118 | - cbcx * cbcz*(calx*saly*(cbly*sblz - cblz * sblx*sbly) 119 | - calx * caly*(sbly*sblz + cbly * cblz*sblx) + cblx * cblz*salx) 120 | - cbcx * sbcz*(calx*caly*(cblz*sbly - cbly * sblx*sblz) 121 | - calx * saly*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sblz); 122 | acx = -asin(srx); 123 | 124 | float srycrx = (cbcy*sbcz - cbcz * sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz * sblx*sbly) 125 | - calx * caly*(sbly*sblz + cbly * cblz*sblx) + cblx * cblz*salx) 126 | - (cbcy*cbcz + sbcx * sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly * sblx*sblz) 127 | - calx * saly*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sblz) 128 | + cbcx * sbcy*(salx*sblx + calx * caly*cblx*cbly + calx * cblx*saly*sbly); 129 | float crycrx = (cbcz*sbcy - cbcy * sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly * sblx*sblz) 130 | - calx * saly*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sblz) 131 | - (sbcy*sbcz + cbcy * cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz * sblx*sbly) 132 | - calx * caly*(sbly*sblz + cbly * cblz*sblx) + cblx * cblz*salx) 133 | + cbcx * cbcy*(salx*sblx + calx * caly*cblx*cbly + calx * cblx*saly*sbly); 134 | acy = atan2(srycrx / acx.cos(), crycrx / acx.cos()); 135 | 136 | float srzcrx = sbcx * (cblx*cbly*(calz*saly - caly * salx*salz) - cblx * sbly*(caly*calz + salx * saly*salz) + calx * salz*sblx) 137 | - cbcx * cbcz*((caly*calz + salx * saly*salz)*(cbly*sblz - cblz * sblx*sbly) 138 | + (calz*saly - caly * salx*salz)*(sbly*sblz + cbly * cblz*sblx) 139 | - calx * cblx*cblz*salz) 140 | + cbcx * sbcz*((caly*calz + salx * saly*salz)*(cbly*cblz + sblx * sbly*sblz) 141 | + (calz*saly - caly * salx*salz)*(cblz*sbly - cbly * sblx*sblz) 142 | + calx * cblx*salz*sblz); 143 | float crzcrx = sbcx * (cblx*sbly*(caly*salz - calz * salx*saly) - cblx * cbly*(saly*salz + caly * calz*salx) + calx * calz*sblx) 144 | + cbcx * cbcz*((saly*salz + caly * calz*salx)*(sbly*sblz + cbly * cblz*sblx) 145 | + (caly*salz - calz * salx*saly)*(cbly*sblz - cblz * sblx*sbly) 146 | + calx * calz*cblx*cblz) 147 | - cbcx * sbcz*((saly*salz + caly * calz*salx)*(cblz*sbly - cbly * sblx*sblz) 148 | + (caly*salz - calz * salx*saly)*(cbly*cblz + sblx * sbly*sblz) 149 | - calx * calz*cblx*sblz); 150 | acz = atan2(srzcrx / acx.cos(), crzcrx / acx.cos()); 151 | } 152 | 153 | 154 | 155 | void BasicLaserOdometry::accumulateRotation(Angle cx, Angle cy, Angle cz, 156 | Angle lx, Angle ly, Angle lz, 157 | Angle &ox, Angle &oy, Angle &oz) 158 | { 159 | float srx = lx.cos()*cx.cos()*ly.sin()*cz.sin() 160 | - cx.cos()*cz.cos()*lx.sin() 161 | - lx.cos()*ly.cos()*cx.sin(); 162 | ox = -asin(srx); 163 | 164 | float srycrx = lx.sin()*(cy.cos()*cz.sin() - cz.cos()*cx.sin()*cy.sin()) 165 | + lx.cos()*ly.sin()*(cy.cos()*cz.cos() + cx.sin()*cy.sin()*cz.sin()) 166 | + lx.cos()*ly.cos()*cx.cos()*cy.sin(); 167 | float crycrx = lx.cos()*ly.cos()*cx.cos()*cy.cos() 168 | - lx.cos()*ly.sin()*(cz.cos()*cy.sin() - cy.cos()*cx.sin()*cz.sin()) 169 | - lx.sin()*(cy.sin()*cz.sin() + cy.cos()*cz.cos()*cx.sin()); 170 | oy = atan2(srycrx / ox.cos(), crycrx / ox.cos()); 171 | 172 | float srzcrx = cx.sin()*(lz.cos()*ly.sin() - ly.cos()*lx.sin()*lz.sin()) 173 | + cx.cos()*cz.sin()*(ly.cos()*lz.cos() + lx.sin()*ly.sin()*lz.sin()) 174 | + lx.cos()*cx.cos()*cz.cos()*lz.sin(); 175 | float crzcrx = lx.cos()*lz.cos()*cx.cos()*cz.cos() 176 | - cx.cos()*cz.sin()*(ly.cos()*lz.sin() - lz.cos()*lx.sin()*ly.sin()) 177 | - cx.sin()*(ly.sin()*lz.sin() + ly.cos()*lz.cos()*lx.sin()); 178 | oz = atan2(srzcrx / ox.cos(), crzcrx / ox.cos()); 179 | } 180 | 181 | void BasicLaserOdometry::updateIMU(pcl::PointCloud const& imuTrans) 182 | { 183 | assert(4 == imuTrans.size()); 184 | _imuPitchStart = imuTrans.points[0].x; 185 | _imuYawStart = imuTrans.points[0].y; 186 | _imuRollStart = imuTrans.points[0].z; 187 | 188 | _imuPitchEnd = imuTrans.points[1].x; 189 | _imuYawEnd = imuTrans.points[1].y; 190 | _imuRollEnd = imuTrans.points[1].z; 191 | 192 | _imuShiftFromStart = imuTrans.points[2]; 193 | _imuVeloFromStart = imuTrans.points[3]; 194 | } 195 | 196 | void BasicLaserOdometry::process() 197 | { 198 | if (!_systemInited) 199 | { 200 | _cornerPointsLessSharp.swap(_lastCornerCloud); 201 | _surfPointsLessFlat.swap(_lastSurfaceCloud); 202 | 203 | _lastCornerKDTree.setInputCloud(_lastCornerCloud); 204 | _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud); 205 | 206 | _transformSum.rot_x += _imuPitchStart; 207 | _transformSum.rot_z += _imuRollStart; 208 | 209 | _systemInited = true; 210 | return; 211 | } 212 | 213 | pcl::PointXYZI coeff; 214 | bool isDegenerate = false; 215 | Eigen::Matrix matP; 216 | 217 | _frameCount++; 218 | _transform.pos -= _imuVeloFromStart * _scanPeriod; 219 | 220 | 221 | size_t lastCornerCloudSize = _lastCornerCloud->points.size(); 222 | size_t lastSurfaceCloudSize = _lastSurfaceCloud->points.size(); 223 | 224 | if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100) 225 | { 226 | std::vector pointSearchInd(1); 227 | std::vector pointSearchSqDis(1); 228 | std::vector indices; 229 | 230 | pcl::removeNaNFromPointCloud(*_cornerPointsSharp, *_cornerPointsSharp, indices); 231 | size_t cornerPointsSharpNum = _cornerPointsSharp->points.size(); 232 | size_t surfPointsFlatNum = _surfPointsFlat->points.size(); 233 | 234 | _pointSearchCornerInd1.resize(cornerPointsSharpNum); 235 | _pointSearchCornerInd2.resize(cornerPointsSharpNum); 236 | _pointSearchSurfInd1.resize(surfPointsFlatNum); 237 | _pointSearchSurfInd2.resize(surfPointsFlatNum); 238 | _pointSearchSurfInd3.resize(surfPointsFlatNum); 239 | 240 | for (size_t iterCount = 0; iterCount < _maxIterations; iterCount++) 241 | { 242 | pcl::PointXYZI pointSel, pointProj, tripod1, tripod2, tripod3; 243 | _laserCloudOri->clear(); 244 | _coeffSel->clear(); 245 | 246 | for (int i = 0; i < cornerPointsSharpNum; i++) 247 | { 248 | transformToStart(_cornerPointsSharp->points[i], pointSel); 249 | 250 | if (iterCount % 5 == 0) 251 | { 252 | pcl::removeNaNFromPointCloud(*_lastCornerCloud, *_lastCornerCloud, indices); 253 | _lastCornerKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); 254 | 255 | int closestPointInd = -1, minPointInd2 = -1; 256 | if (pointSearchSqDis[0] < 25) 257 | { 258 | closestPointInd = pointSearchInd[0]; 259 | int closestPointScan = int(_lastCornerCloud->points[closestPointInd].intensity); 260 | 261 | float pointSqDis, minPointSqDis2 = 25; 262 | for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) 263 | { 264 | if (int(_lastCornerCloud->points[j].intensity) > closestPointScan + 2.5) 265 | { 266 | break; 267 | } 268 | 269 | pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel); 270 | 271 | if (int(_lastCornerCloud->points[j].intensity) > closestPointScan) 272 | { 273 | if (pointSqDis < minPointSqDis2) 274 | { 275 | minPointSqDis2 = pointSqDis; 276 | minPointInd2 = j; 277 | } 278 | } 279 | } 280 | for (int j = closestPointInd - 1; j >= 0; j--) 281 | { 282 | if (int(_lastCornerCloud->points[j].intensity) < closestPointScan - 2.5) 283 | { 284 | break; 285 | } 286 | 287 | pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel); 288 | 289 | if (int(_lastCornerCloud->points[j].intensity) < closestPointScan) 290 | { 291 | if (pointSqDis < minPointSqDis2) 292 | { 293 | minPointSqDis2 = pointSqDis; 294 | minPointInd2 = j; 295 | } 296 | } 297 | } 298 | } 299 | 300 | _pointSearchCornerInd1[i] = closestPointInd; 301 | _pointSearchCornerInd2[i] = minPointInd2; 302 | } 303 | 304 | if (_pointSearchCornerInd2[i] >= 0) 305 | { 306 | tripod1 = _lastCornerCloud->points[_pointSearchCornerInd1[i]]; 307 | tripod2 = _lastCornerCloud->points[_pointSearchCornerInd2[i]]; 308 | 309 | float x0 = pointSel.x; 310 | float y0 = pointSel.y; 311 | float z0 = pointSel.z; 312 | float x1 = tripod1.x; 313 | float y1 = tripod1.y; 314 | float z1 = tripod1.z; 315 | float x2 = tripod2.x; 316 | float y2 = tripod2.y; 317 | float z2 = tripod2.z; 318 | 319 | float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 320 | * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 321 | + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 322 | * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 323 | + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) 324 | * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); 325 | 326 | float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); 327 | 328 | float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 329 | + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; 330 | 331 | float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 332 | - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; 333 | 334 | float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 335 | + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; 336 | 337 | float ld2 = a012 / l12; // Eq. (2) 338 | 339 | // TODO: Why writing to a variable that's never read? 340 | pointProj = pointSel; 341 | pointProj.x -= la * ld2; 342 | pointProj.y -= lb * ld2; 343 | pointProj.z -= lc * ld2; 344 | 345 | float s = 1; 346 | if (iterCount >= 5) 347 | { 348 | s = 1 - 1.8f * fabs(ld2); 349 | } 350 | 351 | coeff.x = s * la; 352 | coeff.y = s * lb; 353 | coeff.z = s * lc; 354 | coeff.intensity = s * ld2; 355 | 356 | if (s > 0.1 && ld2 != 0) 357 | { 358 | _laserCloudOri->push_back(_cornerPointsSharp->points[i]); 359 | _coeffSel->push_back(coeff); 360 | } 361 | } 362 | } 363 | 364 | for (int i = 0; i < surfPointsFlatNum; i++) 365 | { 366 | transformToStart(_surfPointsFlat->points[i], pointSel); 367 | 368 | if (iterCount % 5 == 0) 369 | { 370 | _lastSurfaceKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); 371 | int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; 372 | if (pointSearchSqDis[0] < 25) 373 | { 374 | closestPointInd = pointSearchInd[0]; 375 | int closestPointScan = int(_lastSurfaceCloud->points[closestPointInd].intensity); 376 | 377 | float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25; 378 | for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) 379 | { 380 | if (int(_lastSurfaceCloud->points[j].intensity) > closestPointScan + 2.5) 381 | { 382 | break; 383 | } 384 | 385 | pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel); 386 | 387 | if (int(_lastSurfaceCloud->points[j].intensity) <= closestPointScan) 388 | { 389 | if (pointSqDis < minPointSqDis2) 390 | { 391 | minPointSqDis2 = pointSqDis; 392 | minPointInd2 = j; 393 | } 394 | } 395 | else 396 | { 397 | if (pointSqDis < minPointSqDis3) 398 | { 399 | minPointSqDis3 = pointSqDis; 400 | minPointInd3 = j; 401 | } 402 | } 403 | } 404 | for (int j = closestPointInd - 1; j >= 0; j--) 405 | { 406 | if (int(_lastSurfaceCloud->points[j].intensity) < closestPointScan - 2.5) 407 | { 408 | break; 409 | } 410 | 411 | pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel); 412 | 413 | if (int(_lastSurfaceCloud->points[j].intensity) >= closestPointScan) 414 | { 415 | if (pointSqDis < minPointSqDis2) 416 | { 417 | minPointSqDis2 = pointSqDis; 418 | minPointInd2 = j; 419 | } 420 | } 421 | else 422 | { 423 | if (pointSqDis < minPointSqDis3) 424 | { 425 | minPointSqDis3 = pointSqDis; 426 | minPointInd3 = j; 427 | } 428 | } 429 | } 430 | } 431 | 432 | _pointSearchSurfInd1[i] = closestPointInd; 433 | _pointSearchSurfInd2[i] = minPointInd2; 434 | _pointSearchSurfInd3[i] = minPointInd3; 435 | } 436 | 437 | if (_pointSearchSurfInd2[i] >= 0 && _pointSearchSurfInd3[i] >= 0) 438 | { 439 | tripod1 = _lastSurfaceCloud->points[_pointSearchSurfInd1[i]]; 440 | tripod2 = _lastSurfaceCloud->points[_pointSearchSurfInd2[i]]; 441 | tripod3 = _lastSurfaceCloud->points[_pointSearchSurfInd3[i]]; 442 | 443 | float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) 444 | - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z); 445 | float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) 446 | - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x); 447 | float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) 448 | - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y); 449 | float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z); 450 | 451 | float ps = sqrt(pa * pa + pb * pb + pc * pc); 452 | pa /= ps; 453 | pb /= ps; 454 | pc /= ps; 455 | pd /= ps; 456 | 457 | float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; //Eq. (3)?? 458 | 459 | // TODO: Why writing to a variable that's never read? Maybe it should be used afterwards? 460 | pointProj = pointSel; 461 | pointProj.x -= pa * pd2; 462 | pointProj.y -= pb * pd2; 463 | pointProj.z -= pc * pd2; 464 | 465 | float s = 1; 466 | if (iterCount >= 5) 467 | { 468 | s = 1 - 1.8f * fabs(pd2) / sqrt(calcPointDistance(pointSel)); 469 | } 470 | 471 | coeff.x = s * pa; 472 | coeff.y = s * pb; 473 | coeff.z = s * pc; 474 | coeff.intensity = s * pd2; 475 | 476 | if (s > 0.1 && pd2 != 0) 477 | { 478 | _laserCloudOri->push_back(_surfPointsFlat->points[i]); 479 | _coeffSel->push_back(coeff); 480 | } 481 | } 482 | } 483 | 484 | int pointSelNum = _laserCloudOri->points.size(); 485 | if (pointSelNum < 10) 486 | { 487 | continue; 488 | } 489 | 490 | Eigen::Matrix matA(pointSelNum, 6); 491 | Eigen::Matrix matAt(6, pointSelNum); 492 | Eigen::Matrix matAtA; 493 | Eigen::VectorXf matB(pointSelNum); 494 | Eigen::Matrix matAtB; 495 | Eigen::Matrix matX; 496 | 497 | for (int i = 0; i < pointSelNum; i++) 498 | { 499 | const pcl::PointXYZI& pointOri = _laserCloudOri->points[i]; 500 | coeff = _coeffSel->points[i]; 501 | 502 | float s = 1; 503 | 504 | float srx = sin(s * _transform.rot_x.rad()); 505 | float crx = cos(s * _transform.rot_x.rad()); 506 | float sry = sin(s * _transform.rot_y.rad()); 507 | float cry = cos(s * _transform.rot_y.rad()); 508 | float srz = sin(s * _transform.rot_z.rad()); 509 | float crz = cos(s * _transform.rot_z.rad()); 510 | float tx = s * _transform.pos.x(); 511 | float ty = s * _transform.pos.y(); 512 | float tz = s * _transform.pos.z(); 513 | 514 | float arx = (-s * crx*sry*srz*pointOri.x + s * crx*crz*sry*pointOri.y + s * srx*sry*pointOri.z 515 | + s * tx*crx*sry*srz - s * ty*crx*crz*sry - s * tz*srx*sry) * coeff.x 516 | + (s*srx*srz*pointOri.x - s * crz*srx*pointOri.y + s * crx*pointOri.z 517 | + s * ty*crz*srx - s * tz*crx - s * tx*srx*srz) * coeff.y 518 | + (s*crx*cry*srz*pointOri.x - s * crx*cry*crz*pointOri.y - s * cry*srx*pointOri.z 519 | + s * tz*cry*srx + s * ty*crx*cry*crz - s * tx*crx*cry*srz) * coeff.z; 520 | 521 | float ary = ((-s * crz*sry - s * cry*srx*srz)*pointOri.x 522 | + (s*cry*crz*srx - s * sry*srz)*pointOri.y - s * crx*cry*pointOri.z 523 | + tx * (s*crz*sry + s * cry*srx*srz) + ty * (s*sry*srz - s * cry*crz*srx) 524 | + s * tz*crx*cry) * coeff.x 525 | + ((s*cry*crz - s * srx*sry*srz)*pointOri.x 526 | + (s*cry*srz + s * crz*srx*sry)*pointOri.y - s * crx*sry*pointOri.z 527 | + s * tz*crx*sry - ty * (s*cry*srz + s * crz*srx*sry) 528 | - tx * (s*cry*crz - s * srx*sry*srz)) * coeff.z; 529 | 530 | float arz = ((-s * cry*srz - s * crz*srx*sry)*pointOri.x + (s*cry*crz - s * srx*sry*srz)*pointOri.y 531 | + tx * (s*cry*srz + s * crz*srx*sry) - ty * (s*cry*crz - s * srx*sry*srz)) * coeff.x 532 | + (-s * crx*crz*pointOri.x - s * crx*srz*pointOri.y 533 | + s * ty*crx*srz + s * tx*crx*crz) * coeff.y 534 | + ((s*cry*crz*srx - s * sry*srz)*pointOri.x + (s*crz*sry + s * cry*srx*srz)*pointOri.y 535 | + tx * (s*sry*srz - s * cry*crz*srx) - ty * (s*crz*sry + s * cry*srx*srz)) * coeff.z; 536 | 537 | float atx = -s * (cry*crz - srx * sry*srz) * coeff.x + s * crx*srz * coeff.y 538 | - s * (crz*sry + cry * srx*srz) * coeff.z; 539 | 540 | float aty = -s * (cry*srz + crz * srx*sry) * coeff.x - s * crx*crz * coeff.y 541 | - s * (sry*srz - cry * crz*srx) * coeff.z; 542 | 543 | float atz = s * crx*sry * coeff.x - s * srx * coeff.y - s * crx*cry * coeff.z; 544 | 545 | float d2 = coeff.intensity; 546 | 547 | matA(i, 0) = arx; 548 | matA(i, 1) = ary; 549 | matA(i, 2) = arz; 550 | matA(i, 3) = atx; 551 | matA(i, 4) = aty; 552 | matA(i, 5) = atz; 553 | matB(i, 0) = -0.05 * d2; 554 | } 555 | matAt = matA.transpose(); 556 | matAtA = matAt * matA; 557 | matAtB = matAt * matB; 558 | 559 | matX = matAtA.colPivHouseholderQr().solve(matAtB); 560 | 561 | if (iterCount == 0) 562 | { 563 | Eigen::Matrix matE; 564 | Eigen::Matrix matV; 565 | Eigen::Matrix matV2; 566 | 567 | Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA); 568 | matE = esolver.eigenvalues().real(); 569 | matV = esolver.eigenvectors().real(); 570 | 571 | matV2 = matV; 572 | 573 | isDegenerate = false; 574 | float eignThre[6] = { 10, 10, 10, 10, 10, 10 }; 575 | for (int i = 0; i < 6; i++) 576 | { 577 | if (matE(0, i) < eignThre[i]) 578 | { 579 | for (int j = 0; j < 6; j++) 580 | { 581 | matV2(i, j) = 0; 582 | } 583 | isDegenerate = true; 584 | } 585 | else 586 | { 587 | break; 588 | } 589 | } 590 | matP = matV.inverse() * matV2; 591 | } 592 | 593 | if (isDegenerate) 594 | { 595 | Eigen::Matrix matX2(matX); 596 | matX = matP * matX2; 597 | } 598 | 599 | _transform.rot_x = _transform.rot_x.rad() + matX(0, 0); 600 | _transform.rot_y = _transform.rot_y.rad() + matX(1, 0); 601 | _transform.rot_z = _transform.rot_z.rad() + matX(2, 0); 602 | _transform.pos.x() += matX(3, 0); 603 | _transform.pos.y() += matX(4, 0); 604 | _transform.pos.z() += matX(5, 0); 605 | 606 | if (!pcl_isfinite(_transform.rot_x.rad())) _transform.rot_x = Angle(); 607 | if (!pcl_isfinite(_transform.rot_y.rad())) _transform.rot_y = Angle(); 608 | if (!pcl_isfinite(_transform.rot_z.rad())) _transform.rot_z = Angle(); 609 | 610 | if (!pcl_isfinite(_transform.pos.x())) _transform.pos.x() = 0.0; 611 | if (!pcl_isfinite(_transform.pos.y())) _transform.pos.y() = 0.0; 612 | if (!pcl_isfinite(_transform.pos.z())) _transform.pos.z() = 0.0; 613 | 614 | float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) + 615 | pow(rad2deg(matX(1, 0)), 2) + 616 | pow(rad2deg(matX(2, 0)), 2)); 617 | float deltaT = sqrt(pow(matX(3, 0) * 100, 2) + 618 | pow(matX(4, 0) * 100, 2) + 619 | pow(matX(5, 0) * 100, 2)); 620 | 621 | if (deltaR < _deltaRAbort && deltaT < _deltaTAbort) 622 | break; 623 | } 624 | } 625 | 626 | Angle rx, ry, rz; 627 | accumulateRotation(_transformSum.rot_x, 628 | _transformSum.rot_y, 629 | _transformSum.rot_z, 630 | -_transform.rot_x, 631 | -_transform.rot_y.rad() * 1.05, 632 | -_transform.rot_z, 633 | rx, ry, rz); 634 | 635 | Vector3 v(_transform.pos.x() - _imuShiftFromStart.x(), 636 | _transform.pos.y() - _imuShiftFromStart.y(), 637 | _transform.pos.z() * 1.05 - _imuShiftFromStart.z()); 638 | rotateZXY(v, rz, rx, ry); 639 | Vector3 trans = _transformSum.pos - v; 640 | 641 | pluginIMURotation(rx, ry, rz, 642 | _imuPitchStart, _imuYawStart, _imuRollStart, 643 | _imuPitchEnd, _imuYawEnd, _imuRollEnd, 644 | rx, ry, rz); 645 | 646 | _transformSum.rot_x = rx; 647 | _transformSum.rot_y = ry; 648 | _transformSum.rot_z = rz; 649 | _transformSum.pos = trans; 650 | 651 | transformToEnd(_cornerPointsLessSharp); 652 | transformToEnd(_surfPointsLessFlat); 653 | 654 | _cornerPointsLessSharp.swap(_lastCornerCloud); 655 | _surfPointsLessFlat.swap(_lastSurfaceCloud); 656 | 657 | lastCornerCloudSize = _lastCornerCloud->points.size(); 658 | lastSurfaceCloudSize = _lastSurfaceCloud->points.size(); 659 | 660 | if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100) 661 | { 662 | _lastCornerKDTree.setInputCloud(_lastCornerCloud); 663 | _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud); 664 | } 665 | 666 | } 667 | 668 | 669 | 670 | } // end namespace loam 671 | -------------------------------------------------------------------------------- /src/lib/BasicScanRegistration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "loam_velodyne/BasicScanRegistration.h" 4 | #include "math_utils.h" 5 | 6 | namespace loam 7 | { 8 | 9 | RegistrationParams::RegistrationParams(const float& scanPeriod_, 10 | const int& imuHistorySize_, 11 | const int& nFeatureRegions_, 12 | const int& curvatureRegion_, 13 | const int& maxCornerSharp_, 14 | const int& maxSurfaceFlat_, 15 | const float& lessFlatFilterSize_, 16 | const float& surfaceCurvatureThreshold_) 17 | : scanPeriod(scanPeriod_), 18 | imuHistorySize(imuHistorySize_), 19 | nFeatureRegions(nFeatureRegions_), 20 | curvatureRegion(curvatureRegion_), 21 | maxCornerSharp(maxCornerSharp_), 22 | maxCornerLessSharp(10 * maxCornerSharp_), 23 | maxSurfaceFlat(maxSurfaceFlat_), 24 | lessFlatFilterSize(lessFlatFilterSize_), 25 | surfaceCurvatureThreshold(surfaceCurvatureThreshold_) 26 | {}; 27 | 28 | void BasicScanRegistration::processScanlines(const Time& scanTime, std::vector> const& laserCloudScans) 29 | { 30 | // reset internal buffers and set IMU start state based on current scan time 31 | reset(scanTime); 32 | 33 | // construct sorted full resolution cloud 34 | size_t cloudSize = 0; 35 | for (int i = 0; i < laserCloudScans.size(); i++) { 36 | _laserCloud += laserCloudScans[i]; 37 | 38 | IndexRange range(cloudSize, 0); 39 | cloudSize += laserCloudScans[i].size(); 40 | range.second = cloudSize > 0 ? cloudSize - 1 : 0; 41 | _scanIndices.push_back(range); 42 | } 43 | 44 | extractFeatures(); 45 | updateIMUTransform(); 46 | } 47 | 48 | bool BasicScanRegistration::configure(const RegistrationParams& config) 49 | { 50 | _config = config; 51 | _imuHistory.ensureCapacity(_config.imuHistorySize); 52 | return true; 53 | } 54 | 55 | void BasicScanRegistration::reset(const Time& scanTime) 56 | { 57 | _scanTime = scanTime; 58 | 59 | // re-initialize IMU start index and state 60 | _imuIdx = 0; 61 | if (hasIMUData()) { 62 | interpolateIMUStateFor(0, _imuStart); 63 | } 64 | 65 | // clear internal cloud buffers at the beginning of a sweep 66 | if (true/*newSweep*/) { 67 | _sweepStart = scanTime; 68 | 69 | // clear cloud buffers 70 | _laserCloud.clear(); 71 | _cornerPointsSharp.clear(); 72 | _cornerPointsLessSharp.clear(); 73 | _surfacePointsFlat.clear(); 74 | _surfacePointsLessFlat.clear(); 75 | 76 | // clear scan indices vector 77 | _scanIndices.clear(); 78 | } 79 | } 80 | 81 | 82 | void BasicScanRegistration::updateIMUData(Vector3& acc, IMUState& newState) 83 | { 84 | if (_imuHistory.size() > 0) { 85 | // accumulate IMU position and velocity over time 86 | rotateZXY(acc, newState.roll, newState.pitch, newState.yaw); 87 | 88 | const IMUState& prevState = _imuHistory.last(); 89 | float timeDiff = toSec(newState.stamp - prevState.stamp); 90 | newState.position = prevState.position 91 | + (prevState.velocity * timeDiff) 92 | + (0.5 * acc * timeDiff * timeDiff); 93 | newState.velocity = prevState.velocity 94 | + acc * timeDiff; 95 | } 96 | 97 | _imuHistory.push(newState); 98 | } 99 | 100 | 101 | void BasicScanRegistration::projectPointToStartOfSweep(pcl::PointXYZI& point, float relTime) 102 | { 103 | // project point to the start of the sweep using corresponding IMU data 104 | if (hasIMUData()) 105 | { 106 | setIMUTransformFor(relTime); 107 | transformToStartIMU(point); 108 | } 109 | } 110 | 111 | 112 | void BasicScanRegistration::setIMUTransformFor(const float& relTime) 113 | { 114 | interpolateIMUStateFor(relTime, _imuCur); 115 | 116 | float relSweepTime = toSec(_scanTime - _sweepStart) + relTime; 117 | _imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime; 118 | } 119 | 120 | 121 | 122 | void BasicScanRegistration::transformToStartIMU(pcl::PointXYZI& point) 123 | { 124 | // rotate point to global IMU system 125 | rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw); 126 | 127 | // add global IMU position shift 128 | point.x += _imuPositionShift.x(); 129 | point.y += _imuPositionShift.y(); 130 | point.z += _imuPositionShift.z(); 131 | 132 | // rotate point back to local IMU system relative to the start IMU state 133 | rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll); 134 | } 135 | 136 | 137 | 138 | void BasicScanRegistration::interpolateIMUStateFor(const float &relTime, IMUState &outputState) 139 | { 140 | double timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime; 141 | while (_imuIdx < _imuHistory.size() - 1 && timeDiff > 0) { 142 | _imuIdx++; 143 | timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime; 144 | } 145 | 146 | if (_imuIdx == 0 || timeDiff > 0) { 147 | outputState = _imuHistory[_imuIdx]; 148 | } else { 149 | float ratio = -timeDiff / toSec(_imuHistory[_imuIdx].stamp - _imuHistory[_imuIdx - 1].stamp); 150 | IMUState::interpolate(_imuHistory[_imuIdx], _imuHistory[_imuIdx - 1], ratio, outputState); 151 | } 152 | } 153 | 154 | 155 | void BasicScanRegistration::extractFeatures(const uint16_t& beginIdx) 156 | { 157 | // extract features from individual scans 158 | size_t nScans = _scanIndices.size(); 159 | for (size_t i = beginIdx; i < nScans; i++) { 160 | pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); 161 | size_t scanStartIdx = _scanIndices[i].first; 162 | size_t scanEndIdx = _scanIndices[i].second; 163 | 164 | // skip empty scans 165 | if (scanEndIdx <= scanStartIdx + 2 * _config.curvatureRegion) { 166 | continue; 167 | } 168 | 169 | // Quick&Dirty fix for relative point time calculation without IMU data 170 | /*float scanSize = scanEndIdx - scanStartIdx + 1; 171 | for (int j = scanStartIdx; j <= scanEndIdx; j++) { 172 | _laserCloud[j].intensity = i + _scanPeriod * (j - scanStartIdx) / scanSize; 173 | }*/ 174 | 175 | // reset scan buffers 176 | setScanBuffersFor(scanStartIdx, scanEndIdx); 177 | 178 | // extract features from equally sized scan regions 179 | for (int j = 0; j < _config.nFeatureRegions; j++) { 180 | size_t sp = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - j) 181 | + (scanEndIdx - _config.curvatureRegion) * j) / _config.nFeatureRegions; 182 | size_t ep = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - 1 - j) 183 | + (scanEndIdx - _config.curvatureRegion) * (j + 1)) / _config.nFeatureRegions - 1; 184 | 185 | // skip empty regions 186 | if (ep <= sp) { 187 | continue; 188 | } 189 | 190 | size_t regionSize = ep - sp + 1; 191 | 192 | // reset region buffers 193 | setRegionBuffersFor(sp, ep); 194 | 195 | 196 | // extract corner features 197 | int largestPickedNum = 0; 198 | for (size_t k = regionSize; k > 0 && largestPickedNum < _config.maxCornerLessSharp;) { 199 | size_t idx = _regionSortIndices[--k]; 200 | size_t scanIdx = idx - scanStartIdx; 201 | size_t regionIdx = idx - sp; 202 | 203 | if (_scanNeighborPicked[scanIdx] == 0 && 204 | _regionCurvature[regionIdx] > _config.surfaceCurvatureThreshold) { 205 | 206 | largestPickedNum++; 207 | if (largestPickedNum <= _config.maxCornerSharp) { 208 | _regionLabel[regionIdx] = CORNER_SHARP; 209 | _cornerPointsSharp.push_back(_laserCloud[idx]); 210 | } else { 211 | _regionLabel[regionIdx] = CORNER_LESS_SHARP; 212 | } 213 | _cornerPointsLessSharp.push_back(_laserCloud[idx]); 214 | 215 | markAsPicked(idx, scanIdx); 216 | } 217 | } 218 | 219 | // extract flat surface features 220 | int smallestPickedNum = 0; 221 | for (int k = 0; k < regionSize && smallestPickedNum < _config.maxSurfaceFlat; k++) { 222 | size_t idx = _regionSortIndices[k]; 223 | size_t scanIdx = idx - scanStartIdx; 224 | size_t regionIdx = idx - sp; 225 | 226 | if (_scanNeighborPicked[scanIdx] == 0 && 227 | _regionCurvature[regionIdx] < _config.surfaceCurvatureThreshold) { 228 | 229 | smallestPickedNum++; 230 | _regionLabel[regionIdx] = SURFACE_FLAT; 231 | _surfacePointsFlat.push_back(_laserCloud[idx]); 232 | 233 | markAsPicked(idx, scanIdx); 234 | } 235 | } 236 | 237 | // extract less flat surface features 238 | for (int k = 0; k < regionSize; k++) { 239 | if (_regionLabel[k] <= SURFACE_LESS_FLAT) { 240 | surfPointsLessFlatScan->push_back(_laserCloud[sp + k]); 241 | } 242 | } 243 | } 244 | 245 | // down size less flat surface point cloud of current scan 246 | pcl::PointCloud surfPointsLessFlatScanDS; 247 | pcl::VoxelGrid downSizeFilter; 248 | downSizeFilter.setInputCloud(surfPointsLessFlatScan); 249 | downSizeFilter.setLeafSize(_config.lessFlatFilterSize, _config.lessFlatFilterSize, _config.lessFlatFilterSize); 250 | downSizeFilter.filter(surfPointsLessFlatScanDS); 251 | 252 | _surfacePointsLessFlat += surfPointsLessFlatScanDS; 253 | } 254 | } 255 | 256 | 257 | 258 | void BasicScanRegistration::updateIMUTransform() 259 | { 260 | _imuTrans[0].x = _imuStart.pitch.rad(); 261 | _imuTrans[0].y = _imuStart.yaw.rad(); 262 | _imuTrans[0].z = _imuStart.roll.rad(); 263 | 264 | _imuTrans[1].x = _imuCur.pitch.rad(); 265 | _imuTrans[1].y = _imuCur.yaw.rad(); 266 | _imuTrans[1].z = _imuCur.roll.rad(); 267 | 268 | Vector3 imuShiftFromStart = _imuPositionShift; 269 | rotateYXZ(imuShiftFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll); 270 | 271 | _imuTrans[2].x = imuShiftFromStart.x(); 272 | _imuTrans[2].y = imuShiftFromStart.y(); 273 | _imuTrans[2].z = imuShiftFromStart.z(); 274 | 275 | Vector3 imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity; 276 | rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll); 277 | 278 | _imuTrans[3].x = imuVelocityFromStart.x(); 279 | _imuTrans[3].y = imuVelocityFromStart.y(); 280 | _imuTrans[3].z = imuVelocityFromStart.z(); 281 | } 282 | 283 | 284 | void BasicScanRegistration::setRegionBuffersFor(const size_t& startIdx, const size_t& endIdx) 285 | { 286 | // resize buffers 287 | size_t regionSize = endIdx - startIdx + 1; 288 | _regionCurvature.resize(regionSize); 289 | _regionSortIndices.resize(regionSize); 290 | _regionLabel.assign(regionSize, SURFACE_LESS_FLAT); 291 | 292 | // calculate point curvatures and reset sort indices 293 | float pointWeight = -2 * _config.curvatureRegion; 294 | 295 | for (size_t i = startIdx, regionIdx = 0; i <= endIdx; i++, regionIdx++) { 296 | float diffX = pointWeight * _laserCloud[i].x; 297 | float diffY = pointWeight * _laserCloud[i].y; 298 | float diffZ = pointWeight * _laserCloud[i].z; 299 | 300 | for (int j = 1; j <= _config.curvatureRegion; j++) { 301 | diffX += _laserCloud[i + j].x + _laserCloud[i - j].x; 302 | diffY += _laserCloud[i + j].y + _laserCloud[i - j].y; 303 | diffZ += _laserCloud[i + j].z + _laserCloud[i - j].z; 304 | } 305 | 306 | _regionCurvature[regionIdx] = diffX * diffX + diffY * diffY + diffZ * diffZ; 307 | _regionSortIndices[regionIdx] = i; 308 | } 309 | 310 | // sort point curvatures 311 | for (size_t i = 1; i < regionSize; i++) { 312 | for (size_t j = i; j >= 1; j--) { 313 | if (_regionCurvature[_regionSortIndices[j] - startIdx] < _regionCurvature[_regionSortIndices[j - 1] - startIdx]) { 314 | std::swap(_regionSortIndices[j], _regionSortIndices[j - 1]); 315 | } 316 | } 317 | } 318 | } 319 | 320 | 321 | void BasicScanRegistration::setScanBuffersFor(const size_t& startIdx, const size_t& endIdx) 322 | { 323 | // resize buffers 324 | size_t scanSize = endIdx - startIdx + 1; 325 | _scanNeighborPicked.assign(scanSize, 0); 326 | 327 | // mark unreliable points as picked 328 | for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) { 329 | const pcl::PointXYZI& previousPoint = (_laserCloud[i - 1]); 330 | const pcl::PointXYZI& point = (_laserCloud[i]); 331 | const pcl::PointXYZI& nextPoint = (_laserCloud[i + 1]); 332 | 333 | float diffNext = calcSquaredDiff(nextPoint, point); 334 | 335 | if (diffNext > 0.1) { 336 | float depth1 = calcPointDistance(point); 337 | float depth2 = calcPointDistance(nextPoint); 338 | 339 | if (depth1 > depth2) { 340 | float weighted_distance = std::sqrt(calcSquaredDiff(nextPoint, point, depth2 / depth1)) / depth2; 341 | 342 | if (weighted_distance < 0.1) { 343 | std::fill_n(&_scanNeighborPicked[i - startIdx - _config.curvatureRegion], _config.curvatureRegion + 1, 1); 344 | 345 | continue; 346 | } 347 | } else { 348 | float weighted_distance = std::sqrt(calcSquaredDiff(point, nextPoint, depth1 / depth2)) / depth1; 349 | 350 | if (weighted_distance < 0.1) { 351 | std::fill_n(&_scanNeighborPicked[i - startIdx + 1], _config.curvatureRegion + 1, 1); 352 | } 353 | } 354 | } 355 | 356 | float diffPrevious = calcSquaredDiff(point, previousPoint); 357 | float dis = calcSquaredPointDistance(point); 358 | 359 | if (diffNext > 0.0002 * dis && diffPrevious > 0.0002 * dis) { 360 | _scanNeighborPicked[i - startIdx] = 1; 361 | } 362 | } 363 | } 364 | 365 | 366 | 367 | void BasicScanRegistration::markAsPicked(const size_t& cloudIdx, const size_t& scanIdx) 368 | { 369 | _scanNeighborPicked[scanIdx] = 1; 370 | 371 | for (int i = 1; i <= _config.curvatureRegion; i++) { 372 | if (calcSquaredDiff(_laserCloud[cloudIdx + i], _laserCloud[cloudIdx + i - 1]) > 0.05) { 373 | break; 374 | } 375 | 376 | _scanNeighborPicked[scanIdx + i] = 1; 377 | } 378 | 379 | for (int i = 1; i <= _config.curvatureRegion; i++) { 380 | if (calcSquaredDiff(_laserCloud[cloudIdx - i], _laserCloud[cloudIdx - i + 1]) > 0.05) { 381 | break; 382 | } 383 | 384 | _scanNeighborPicked[scanIdx - i] = 1; 385 | } 386 | } 387 | 388 | 389 | } 390 | -------------------------------------------------------------------------------- /src/lib/BasicTransformMaintenance.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 "loam_velodyne/BasicTransformMaintenance.h" 35 | 36 | 37 | namespace loam 38 | { 39 | 40 | using std::sin; 41 | using std::cos; 42 | using std::asin; 43 | using std::atan2; 44 | 45 | 46 | void BasicTransformMaintenance::updateOdometry(double pitch, double yaw, double roll, double x, double y, double z) 47 | { 48 | _transformSum[0] = pitch; 49 | _transformSum[1] = yaw; 50 | _transformSum[2] = roll; 51 | _transformSum[3] = x; 52 | _transformSum[4] = y; 53 | _transformSum[5] = z; 54 | } 55 | 56 | void BasicTransformMaintenance::updateMappingTransform(double pitch, double yaw, double roll, double x, double y, double z, double twist_rot_x, double twist_rot_y, double twist_rot_z, double twist_pos_x, double twist_pos_y, double twist_pos_z) 57 | { 58 | _transformAftMapped[0] = pitch; 59 | _transformAftMapped[1] = yaw; 60 | _transformAftMapped[2] = roll; 61 | 62 | _transformAftMapped[3] = x; 63 | _transformAftMapped[4] = y; 64 | _transformAftMapped[5] = z; 65 | 66 | _transformBefMapped[0] = twist_rot_x; 67 | _transformBefMapped[1] = twist_rot_y; 68 | _transformBefMapped[2] = twist_rot_z; 69 | 70 | _transformBefMapped[3] = twist_pos_x; 71 | _transformBefMapped[4] = twist_pos_y; 72 | _transformBefMapped[5] = twist_pos_z; 73 | } 74 | 75 | void BasicTransformMaintenance::updateMappingTransform(Twist const& transformAftMapped, Twist const& transformBefMapped) 76 | { 77 | updateMappingTransform( transformAftMapped.rot_x.rad(), transformAftMapped.rot_y.rad(), transformAftMapped.rot_z.rad(), 78 | transformAftMapped.pos.x(), transformAftMapped.pos.y(), transformAftMapped.pos.z(), 79 | transformBefMapped.rot_x.rad(), transformBefMapped.rot_y.rad(), transformBefMapped.rot_z.rad(), 80 | transformBefMapped.pos.x(), transformBefMapped.pos.y(), transformBefMapped.pos.z()); 81 | } 82 | 83 | void BasicTransformMaintenance::transformAssociateToMap() 84 | { 85 | float x1 = cos(_transformSum[1]) * (_transformBefMapped[3] - _transformSum[3]) 86 | - sin(_transformSum[1]) * (_transformBefMapped[5] - _transformSum[5]); 87 | float y1 = _transformBefMapped[4] - _transformSum[4]; 88 | float z1 = sin(_transformSum[1]) * (_transformBefMapped[3] - _transformSum[3]) 89 | + cos(_transformSum[1]) * (_transformBefMapped[5] - _transformSum[5]); 90 | 91 | float x2 = x1; 92 | float y2 = cos(_transformSum[0]) * y1 + sin(_transformSum[0]) * z1; 93 | float z2 = -sin(_transformSum[0]) * y1 + cos(_transformSum[0]) * z1; 94 | 95 | _transformIncre[3] = cos(_transformSum[2]) * x2 + sin(_transformSum[2]) * y2; 96 | _transformIncre[4] = -sin(_transformSum[2]) * x2 + cos(_transformSum[2]) * y2; 97 | _transformIncre[5] = z2; 98 | 99 | float sbcx = sin(_transformSum[0]); 100 | float cbcx = cos(_transformSum[0]); 101 | float sbcy = sin(_transformSum[1]); 102 | float cbcy = cos(_transformSum[1]); 103 | float sbcz = sin(_transformSum[2]); 104 | float cbcz = cos(_transformSum[2]); 105 | 106 | float sblx = sin(_transformBefMapped[0]); 107 | float cblx = cos(_transformBefMapped[0]); 108 | float sbly = sin(_transformBefMapped[1]); 109 | float cbly = cos(_transformBefMapped[1]); 110 | float sblz = sin(_transformBefMapped[2]); 111 | float cblz = cos(_transformBefMapped[2]); 112 | 113 | float salx = sin(_transformAftMapped[0]); 114 | float calx = cos(_transformAftMapped[0]); 115 | float saly = sin(_transformAftMapped[1]); 116 | float caly = cos(_transformAftMapped[1]); 117 | float salz = sin(_transformAftMapped[2]); 118 | float calz = cos(_transformAftMapped[2]); 119 | 120 | float srx = -sbcx * (salx*sblx + calx * cblx*salz*sblz + calx * calz*cblx*cblz) 121 | - cbcx * sbcy*(calx*calz*(cbly*sblz - cblz * sblx*sbly) 122 | - calx * salz*(cbly*cblz + sblx * sbly*sblz) 123 | + cblx * salx*sbly) 124 | - cbcx * cbcy*(calx*salz*(cblz*sbly - cbly * sblx*sblz) 125 | - calx * calz*(sbly*sblz + cbly * cblz*sblx) 126 | + cblx * cbly*salx); 127 | _transformMapped[0] = -asin(srx); 128 | 129 | float srycrx = sbcx * (cblx*cblz*(caly*salz - calz * salx*saly) 130 | - cblx * sblz*(caly*calz + salx * saly*salz) + calx * saly*sblx) 131 | - cbcx * cbcy*((caly*calz + salx * saly*salz)*(cblz*sbly - cbly * sblx*sblz) 132 | + (caly*salz - calz * salx*saly)*(sbly*sblz + cbly * cblz*sblx) 133 | - calx * cblx*cbly*saly) 134 | + cbcx * sbcy*((caly*calz + salx * saly*salz)*(cbly*cblz + sblx * sbly*sblz) 135 | + (caly*salz - calz * salx*saly)*(cbly*sblz - cblz * sblx*sbly) 136 | + calx * cblx*saly*sbly); 137 | float crycrx = sbcx * (cblx*sblz*(calz*saly - caly * salx*salz) 138 | - cblx * cblz*(saly*salz + caly * calz*salx) + calx * caly*sblx) 139 | + cbcx * cbcy*((saly*salz + caly * calz*salx)*(sbly*sblz + cbly * cblz*sblx) 140 | + (calz*saly - caly * salx*salz)*(cblz*sbly - cbly * sblx*sblz) 141 | + calx * caly*cblx*cbly) 142 | - cbcx * sbcy*((saly*salz + caly * calz*salx)*(cbly*sblz - cblz * sblx*sbly) 143 | + (calz*saly - caly * salx*salz)*(cbly*cblz + sblx * sbly*sblz) 144 | - calx * caly*cblx*sbly); 145 | _transformMapped[1] = atan2(srycrx / cos(_transformMapped[0]), 146 | crycrx / cos(_transformMapped[0])); 147 | 148 | float srzcrx = (cbcz*sbcy - cbcy * sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly * sblx*sblz) 149 | - calx * calz*(sbly*sblz + cbly * cblz*sblx) 150 | + cblx * cbly*salx) 151 | - (cbcy*cbcz + sbcx * sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz * sblx*sbly) 152 | - calx * salz*(cbly*cblz + sblx * sbly*sblz) 153 | + cblx * salx*sbly) 154 | + cbcx * sbcz*(salx*sblx + calx * cblx*salz*sblz + calx * calz*cblx*cblz); 155 | float crzcrx = (cbcy*sbcz - cbcz * sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz * sblx*sbly) 156 | - calx * salz*(cbly*cblz + sblx * sbly*sblz) 157 | + cblx * salx*sbly) 158 | - (sbcy*sbcz + cbcy * cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly * sblx*sblz) 159 | - calx * calz*(sbly*sblz + cbly * cblz*sblx) 160 | + cblx * cbly*salx) 161 | + cbcx * cbcz*(salx*sblx + calx * cblx*salz*sblz + calx * calz*cblx*cblz); 162 | _transformMapped[2] = atan2(srzcrx / cos(_transformMapped[0]), 163 | crzcrx / cos(_transformMapped[0])); 164 | 165 | x1 = cos(_transformMapped[2]) * _transformIncre[3] - sin(_transformMapped[2]) * _transformIncre[4]; 166 | y1 = sin(_transformMapped[2]) * _transformIncre[3] + cos(_transformMapped[2]) * _transformIncre[4]; 167 | z1 = _transformIncre[5]; 168 | 169 | x2 = x1; 170 | y2 = cos(_transformMapped[0]) * y1 - sin(_transformMapped[0]) * z1; 171 | z2 = sin(_transformMapped[0]) * y1 + cos(_transformMapped[0]) * z1; 172 | 173 | _transformMapped[3] = _transformAftMapped[3] 174 | - (cos(_transformMapped[1]) * x2 + sin(_transformMapped[1]) * z2); 175 | _transformMapped[4] = _transformAftMapped[4] - y2; 176 | _transformMapped[5] = _transformAftMapped[5] 177 | - (-sin(_transformMapped[1]) * x2 + cos(_transformMapped[1]) * z2); 178 | } 179 | 180 | 181 | } // end namespace loam 182 | -------------------------------------------------------------------------------- /src/lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(loam 2 | math_utils.h 3 | ScanRegistration.cpp 4 | BasicScanRegistration.cpp 5 | MultiScanRegistration.cpp 6 | LaserOdometry.cpp 7 | BasicLaserOdometry.cpp 8 | LaserMapping.cpp 9 | BasicLaserMapping.cpp 10 | TransformMaintenance.cpp 11 | BasicTransformMaintenance.cpp) 12 | target_link_libraries(loam ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 13 | -------------------------------------------------------------------------------- /src/lib/LaserMapping.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 "loam_velodyne/LaserMapping.h" 34 | #include "loam_velodyne/common.h" 35 | 36 | namespace loam 37 | { 38 | 39 | LaserMapping::LaserMapping(const float& scanPeriod, const size_t& maxIterations) 40 | { 41 | // initialize mapping odometry and odometry tf messages 42 | _odomAftMapped.header.frame_id = "/camera_init"; 43 | _odomAftMapped.child_frame_id = "/aft_mapped"; 44 | 45 | _aftMappedTrans.frame_id_ = "/camera_init"; 46 | _aftMappedTrans.child_frame_id_ = "/aft_mapped"; 47 | } 48 | 49 | 50 | bool LaserMapping::setup(ros::NodeHandle& node, ros::NodeHandle& privateNode) 51 | { 52 | // fetch laser mapping params 53 | float fParam; 54 | int iParam; 55 | 56 | if (privateNode.getParam("scanPeriod", fParam)) 57 | { 58 | if (fParam <= 0) 59 | { 60 | ROS_ERROR("Invalid scanPeriod parameter: %f (expected > 0)", fParam); 61 | return false; 62 | } 63 | else 64 | { 65 | setScanPeriod(fParam); 66 | ROS_INFO("Set scanPeriod: %g", fParam); 67 | } 68 | } 69 | 70 | if (privateNode.getParam("maxIterations", iParam)) 71 | { 72 | if (iParam < 1) 73 | { 74 | ROS_ERROR("Invalid maxIterations parameter: %d (expected > 0)", iParam); 75 | return false; 76 | } 77 | else 78 | { 79 | setMaxIterations(iParam); 80 | ROS_INFO("Set maxIterations: %d", iParam); 81 | } 82 | } 83 | 84 | if (privateNode.getParam("deltaTAbort", fParam)) 85 | { 86 | if (fParam <= 0) 87 | { 88 | ROS_ERROR("Invalid deltaTAbort parameter: %f (expected > 0)", fParam); 89 | return false; 90 | } 91 | else 92 | { 93 | setDeltaTAbort(fParam); 94 | ROS_INFO("Set deltaTAbort: %g", fParam); 95 | } 96 | } 97 | 98 | if (privateNode.getParam("deltaRAbort", fParam)) 99 | { 100 | if (fParam <= 0) 101 | { 102 | ROS_ERROR("Invalid deltaRAbort parameter: %f (expected > 0)", fParam); 103 | return false; 104 | } 105 | else 106 | { 107 | setDeltaRAbort(fParam); 108 | ROS_INFO("Set deltaRAbort: %g", fParam); 109 | } 110 | } 111 | 112 | if (privateNode.getParam("cornerFilterSize", fParam)) 113 | { 114 | if (fParam < 0.001) 115 | { 116 | ROS_ERROR("Invalid cornerFilterSize parameter: %f (expected >= 0.001)", fParam); 117 | return false; 118 | } 119 | else 120 | { 121 | downSizeFilterCorner().setLeafSize(fParam, fParam, fParam); 122 | ROS_INFO("Set corner down size filter leaf size: %g", fParam); 123 | } 124 | } 125 | 126 | if (privateNode.getParam("surfaceFilterSize", fParam)) 127 | { 128 | if (fParam < 0.001) 129 | { 130 | ROS_ERROR("Invalid surfaceFilterSize parameter: %f (expected >= 0.001)", fParam); 131 | return false; 132 | } 133 | else 134 | { 135 | downSizeFilterSurf().setLeafSize(fParam, fParam, fParam); 136 | ROS_INFO("Set surface down size filter leaf size: %g", fParam); 137 | } 138 | } 139 | 140 | if (privateNode.getParam("mapFilterSize", fParam)) 141 | { 142 | if (fParam < 0.001) 143 | { 144 | ROS_ERROR("Invalid mapFilterSize parameter: %f (expected >= 0.001)", fParam); 145 | return false; 146 | } 147 | else 148 | { 149 | downSizeFilterMap().setLeafSize(fParam, fParam, fParam); 150 | ROS_INFO("Set map down size filter leaf size: %g", fParam); 151 | } 152 | } 153 | 154 | // advertise laser mapping topics 155 | _pubLaserCloudSurround = node.advertise("/laser_cloud_surround", 1); 156 | _pubLaserCloudFullRes = node.advertise("/velodyne_cloud_registered", 2); 157 | _pubOdomAftMapped = node.advertise("/aft_mapped_to_init", 5); 158 | 159 | // subscribe to laser odometry topics 160 | _subLaserCloudCornerLast = node.subscribe 161 | ("/laser_cloud_corner_last", 2, &LaserMapping::laserCloudCornerLastHandler, this); 162 | 163 | _subLaserCloudSurfLast = node.subscribe 164 | ("/laser_cloud_surf_last", 2, &LaserMapping::laserCloudSurfLastHandler, this); 165 | 166 | _subLaserOdometry = node.subscribe 167 | ("/laser_odom_to_init", 5, &LaserMapping::laserOdometryHandler, this); 168 | 169 | _subLaserCloudFullRes = node.subscribe 170 | ("/velodyne_cloud_3", 2, &LaserMapping::laserCloudFullResHandler, this); 171 | 172 | // subscribe to IMU topic 173 | _subImu = node.subscribe("/imu/data", 50, &LaserMapping::imuHandler, this); 174 | 175 | return true; 176 | } 177 | 178 | 179 | 180 | void LaserMapping::laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLastMsg) 181 | { 182 | _timeLaserCloudCornerLast = cornerPointsLastMsg->header.stamp; 183 | laserCloudCornerLast().clear(); 184 | pcl::fromROSMsg(*cornerPointsLastMsg, laserCloudCornerLast()); 185 | _newLaserCloudCornerLast = true; 186 | } 187 | 188 | void LaserMapping::laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& surfacePointsLastMsg) 189 | { 190 | _timeLaserCloudSurfLast = surfacePointsLastMsg->header.stamp; 191 | laserCloudSurfLast().clear(); 192 | pcl::fromROSMsg(*surfacePointsLastMsg, laserCloudSurfLast()); 193 | _newLaserCloudSurfLast = true; 194 | } 195 | 196 | void LaserMapping::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg) 197 | { 198 | _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp; 199 | laserCloud().clear(); 200 | pcl::fromROSMsg(*laserCloudFullResMsg, laserCloud()); 201 | _newLaserCloudFullRes = true; 202 | } 203 | 204 | void LaserMapping::laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) 205 | { 206 | _timeLaserOdometry = laserOdometry->header.stamp; 207 | 208 | double roll, pitch, yaw; 209 | geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; 210 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 211 | 212 | updateOdometry(-pitch, -yaw, roll, 213 | laserOdometry->pose.pose.position.x, 214 | laserOdometry->pose.pose.position.y, 215 | laserOdometry->pose.pose.position.z); 216 | 217 | _newLaserOdometry = true; 218 | } 219 | 220 | void LaserMapping::imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) 221 | { 222 | double roll, pitch, yaw; 223 | tf::Quaternion orientation; 224 | tf::quaternionMsgToTF(imuIn->orientation, orientation); 225 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); 226 | updateIMU({ fromROSTime(imuIn->header.stamp) , roll, pitch }); 227 | } 228 | 229 | void LaserMapping::spin() 230 | { 231 | ros::Rate rate(100); 232 | bool status = ros::ok(); 233 | 234 | while (status) 235 | { 236 | ros::spinOnce(); 237 | 238 | // try processing buffered data 239 | process(); 240 | 241 | status = ros::ok(); 242 | rate.sleep(); 243 | } 244 | } 245 | 246 | void LaserMapping::reset() 247 | { 248 | _newLaserCloudCornerLast = false; 249 | _newLaserCloudSurfLast = false; 250 | _newLaserCloudFullRes = false; 251 | _newLaserOdometry = false; 252 | } 253 | 254 | bool LaserMapping::hasNewData() 255 | { 256 | return _newLaserCloudCornerLast && _newLaserCloudSurfLast && 257 | _newLaserCloudFullRes && _newLaserOdometry && 258 | fabs((_timeLaserCloudCornerLast - _timeLaserOdometry).toSec()) < 0.005 && 259 | fabs((_timeLaserCloudSurfLast - _timeLaserOdometry).toSec()) < 0.005 && 260 | fabs((_timeLaserCloudFullRes - _timeLaserOdometry).toSec()) < 0.005; 261 | } 262 | 263 | void LaserMapping::process() 264 | { 265 | if (!hasNewData())// waiting for new data to arrive... 266 | return; 267 | 268 | reset();// reset flags, etc. 269 | 270 | if (!BasicLaserMapping::process(fromROSTime(_timeLaserOdometry))) 271 | return; 272 | 273 | publishResult(); 274 | } 275 | 276 | void LaserMapping::publishResult() 277 | { 278 | // publish new map cloud according to the input output ratio 279 | if (hasFreshMap()) // publish new map cloud 280 | publishCloudMsg(_pubLaserCloudSurround, laserCloudSurroundDS(), _timeLaserOdometry, "/camera_init"); 281 | 282 | // publish transformed full resolution input cloud 283 | publishCloudMsg(_pubLaserCloudFullRes, laserCloud(), _timeLaserOdometry, "/camera_init"); 284 | 285 | // publish odometry after mapped transformations 286 | geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw 287 | (transformAftMapped().rot_z.rad(), -transformAftMapped().rot_x.rad(), -transformAftMapped().rot_y.rad()); 288 | 289 | _odomAftMapped.header.stamp = _timeLaserOdometry; 290 | _odomAftMapped.pose.pose.orientation.x = -geoQuat.y; 291 | _odomAftMapped.pose.pose.orientation.y = -geoQuat.z; 292 | _odomAftMapped.pose.pose.orientation.z = geoQuat.x; 293 | _odomAftMapped.pose.pose.orientation.w = geoQuat.w; 294 | _odomAftMapped.pose.pose.position.x = transformAftMapped().pos.x(); 295 | _odomAftMapped.pose.pose.position.y = transformAftMapped().pos.y(); 296 | _odomAftMapped.pose.pose.position.z = transformAftMapped().pos.z(); 297 | _odomAftMapped.twist.twist.angular.x = transformBefMapped().rot_x.rad(); 298 | _odomAftMapped.twist.twist.angular.y = transformBefMapped().rot_y.rad(); 299 | _odomAftMapped.twist.twist.angular.z = transformBefMapped().rot_z.rad(); 300 | _odomAftMapped.twist.twist.linear.x = transformBefMapped().pos.x(); 301 | _odomAftMapped.twist.twist.linear.y = transformBefMapped().pos.y(); 302 | _odomAftMapped.twist.twist.linear.z = transformBefMapped().pos.z(); 303 | _pubOdomAftMapped.publish(_odomAftMapped); 304 | 305 | _aftMappedTrans.stamp_ = _timeLaserOdometry; 306 | _aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 307 | _aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped().pos.x(), 308 | transformAftMapped().pos.y(), 309 | transformAftMapped().pos.z())); 310 | _tfBroadcaster.sendTransform(_aftMappedTrans); 311 | } 312 | 313 | } // end namespace loam 314 | -------------------------------------------------------------------------------- /src/lib/LaserOdometry.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 "loam_velodyne/LaserOdometry.h" 36 | #include "loam_velodyne/common.h" 37 | #include "math_utils.h" 38 | 39 | namespace loam 40 | { 41 | 42 | using std::sin; 43 | using std::cos; 44 | using std::asin; 45 | using std::atan2; 46 | using std::sqrt; 47 | using std::fabs; 48 | using std::pow; 49 | 50 | 51 | LaserOdometry::LaserOdometry(float scanPeriod, uint16_t ioRatio, size_t maxIterations): 52 | BasicLaserOdometry(scanPeriod, maxIterations), 53 | _ioRatio(ioRatio) 54 | { 55 | // initialize odometry and odometry tf messages 56 | _laserOdometryMsg.header.frame_id = "/camera_init"; 57 | _laserOdometryMsg.child_frame_id = "/laser_odom"; 58 | 59 | _laserOdometryTrans.frame_id_ = "/camera_init"; 60 | _laserOdometryTrans.child_frame_id_ = "/laser_odom"; 61 | } 62 | 63 | 64 | bool LaserOdometry::setup(ros::NodeHandle &node, ros::NodeHandle &privateNode) 65 | { 66 | // fetch laser odometry params 67 | float fParam; 68 | int iParam; 69 | 70 | if (privateNode.getParam("scanPeriod", fParam)) 71 | { 72 | if (fParam <= 0) 73 | { 74 | ROS_ERROR("Invalid scanPeriod parameter: %f (expected > 0)", fParam); 75 | return false; 76 | } 77 | else 78 | { 79 | setScanPeriod(fParam); 80 | ROS_INFO("Set scanPeriod: %g", fParam); 81 | } 82 | } 83 | 84 | if (privateNode.getParam("ioRatio", iParam)) 85 | { 86 | if (iParam < 1) 87 | { 88 | ROS_ERROR("Invalid ioRatio parameter: %d (expected > 0)", iParam); 89 | return false; 90 | } 91 | else 92 | { 93 | _ioRatio = iParam; 94 | ROS_INFO("Set ioRatio: %d", iParam); 95 | } 96 | } 97 | 98 | if (privateNode.getParam("maxIterations", iParam)) 99 | { 100 | if (iParam < 1) 101 | { 102 | ROS_ERROR("Invalid maxIterations parameter: %d (expected > 0)", iParam); 103 | return false; 104 | } 105 | else 106 | { 107 | setMaxIterations(iParam); 108 | ROS_INFO("Set maxIterations: %d", iParam); 109 | } 110 | } 111 | 112 | if (privateNode.getParam("deltaTAbort", fParam)) 113 | { 114 | if (fParam <= 0) 115 | { 116 | ROS_ERROR("Invalid deltaTAbort parameter: %f (expected > 0)", fParam); 117 | return false; 118 | } 119 | else 120 | { 121 | setDeltaTAbort(fParam); 122 | ROS_INFO("Set deltaTAbort: %g", fParam); 123 | } 124 | } 125 | 126 | if (privateNode.getParam("deltaRAbort", fParam)) 127 | { 128 | if (fParam <= 0) 129 | { 130 | ROS_ERROR("Invalid deltaRAbort parameter: %f (expected > 0)", fParam); 131 | return false; 132 | } 133 | else 134 | { 135 | setDeltaRAbort(fParam); 136 | ROS_INFO("Set deltaRAbort: %g", fParam); 137 | } 138 | } 139 | 140 | // advertise laser odometry topics 141 | _pubLaserCloudCornerLast = node.advertise("/laser_cloud_corner_last", 2); 142 | _pubLaserCloudSurfLast = node.advertise("/laser_cloud_surf_last", 2); 143 | _pubLaserCloudFullRes = node.advertise("/velodyne_cloud_3", 2); 144 | _pubLaserOdometry = node.advertise("/laser_odom_to_init", 5); 145 | 146 | // subscribe to scan registration topics 147 | _subCornerPointsSharp = node.subscribe 148 | ("/laser_cloud_sharp", 2, &LaserOdometry::laserCloudSharpHandler, this); 149 | 150 | _subCornerPointsLessSharp = node.subscribe 151 | ("/laser_cloud_less_sharp", 2, &LaserOdometry::laserCloudLessSharpHandler, this); 152 | 153 | _subSurfPointsFlat = node.subscribe 154 | ("/laser_cloud_flat", 2, &LaserOdometry::laserCloudFlatHandler, this); 155 | 156 | _subSurfPointsLessFlat = node.subscribe 157 | ("/laser_cloud_less_flat", 2, &LaserOdometry::laserCloudLessFlatHandler, this); 158 | 159 | _subLaserCloudFullRes = node.subscribe 160 | ("/velodyne_cloud_2", 2, &LaserOdometry::laserCloudFullResHandler, this); 161 | 162 | _subImuTrans = node.subscribe 163 | ("/imu_trans", 5, &LaserOdometry::imuTransHandler, this); 164 | 165 | return true; 166 | } 167 | 168 | void LaserOdometry::reset() 169 | { 170 | _newCornerPointsSharp = false; 171 | _newCornerPointsLessSharp = false; 172 | _newSurfPointsFlat = false; 173 | _newSurfPointsLessFlat = false; 174 | _newLaserCloudFullRes = false; 175 | _newImuTrans = false; 176 | } 177 | 178 | void LaserOdometry::laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharpMsg) 179 | { 180 | _timeCornerPointsSharp = cornerPointsSharpMsg->header.stamp; 181 | 182 | cornerPointsSharp()->clear(); 183 | pcl::fromROSMsg(*cornerPointsSharpMsg, *cornerPointsSharp()); 184 | std::vector indices; 185 | pcl::removeNaNFromPointCloud(*cornerPointsSharp(), *cornerPointsSharp(), indices); 186 | _newCornerPointsSharp = true; 187 | } 188 | 189 | 190 | 191 | void LaserOdometry::laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharpMsg) 192 | { 193 | _timeCornerPointsLessSharp = cornerPointsLessSharpMsg->header.stamp; 194 | 195 | cornerPointsLessSharp()->clear(); 196 | pcl::fromROSMsg(*cornerPointsLessSharpMsg, *cornerPointsLessSharp()); 197 | std::vector indices; 198 | pcl::removeNaNFromPointCloud(*cornerPointsLessSharp(), *cornerPointsLessSharp(), indices); 199 | _newCornerPointsLessSharp = true; 200 | } 201 | 202 | 203 | 204 | void LaserOdometry::laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlatMsg) 205 | { 206 | _timeSurfPointsFlat = surfPointsFlatMsg->header.stamp; 207 | 208 | surfPointsFlat()->clear(); 209 | pcl::fromROSMsg(*surfPointsFlatMsg, *surfPointsFlat()); 210 | std::vector indices; 211 | pcl::removeNaNFromPointCloud(*surfPointsFlat(), *surfPointsFlat(), indices); 212 | _newSurfPointsFlat = true; 213 | } 214 | 215 | 216 | 217 | void LaserOdometry::laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlatMsg) 218 | { 219 | _timeSurfPointsLessFlat = surfPointsLessFlatMsg->header.stamp; 220 | 221 | surfPointsLessFlat()->clear(); 222 | pcl::fromROSMsg(*surfPointsLessFlatMsg, *surfPointsLessFlat()); 223 | std::vector indices; 224 | pcl::removeNaNFromPointCloud(*surfPointsLessFlat(), *surfPointsLessFlat(), indices); 225 | _newSurfPointsLessFlat = true; 226 | } 227 | 228 | 229 | 230 | void LaserOdometry::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg) 231 | { 232 | _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp; 233 | 234 | laserCloud()->clear(); 235 | pcl::fromROSMsg(*laserCloudFullResMsg, *laserCloud()); 236 | std::vector indices; 237 | pcl::removeNaNFromPointCloud(*laserCloud(), *laserCloud(), indices); 238 | _newLaserCloudFullRes = true; 239 | } 240 | 241 | 242 | 243 | void LaserOdometry::imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTransMsg) 244 | { 245 | _timeImuTrans = imuTransMsg->header.stamp; 246 | 247 | pcl::PointCloud imuTrans; 248 | pcl::fromROSMsg(*imuTransMsg, imuTrans); 249 | updateIMU(imuTrans); 250 | _newImuTrans = true; 251 | } 252 | 253 | 254 | void LaserOdometry::spin() 255 | { 256 | ros::Rate rate(100); 257 | bool status = ros::ok(); 258 | 259 | // loop until shutdown 260 | while (status) 261 | { 262 | ros::spinOnce(); 263 | 264 | // try processing new data 265 | process(); 266 | 267 | status = ros::ok(); 268 | rate.sleep(); 269 | } 270 | } 271 | 272 | 273 | bool LaserOdometry::hasNewData() 274 | { 275 | return _newCornerPointsSharp && _newCornerPointsLessSharp && _newSurfPointsFlat && 276 | _newSurfPointsLessFlat && _newLaserCloudFullRes && _newImuTrans && 277 | fabs((_timeCornerPointsSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 && 278 | fabs((_timeCornerPointsLessSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 && 279 | fabs((_timeSurfPointsFlat - _timeSurfPointsLessFlat).toSec()) < 0.005 && 280 | fabs((_timeLaserCloudFullRes - _timeSurfPointsLessFlat).toSec()) < 0.005 && 281 | fabs((_timeImuTrans - _timeSurfPointsLessFlat).toSec()) < 0.005; 282 | } 283 | 284 | 285 | 286 | void LaserOdometry::process() 287 | { 288 | if (!hasNewData()) 289 | return;// waiting for new data to arrive... 290 | 291 | reset();// reset flags, etc. 292 | BasicLaserOdometry::process(); 293 | publishResult(); 294 | } 295 | 296 | 297 | void LaserOdometry::publishResult() 298 | { 299 | // publish odometry transformations 300 | geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum().rot_z.rad(), 301 | -transformSum().rot_x.rad(), 302 | -transformSum().rot_y.rad()); 303 | 304 | _laserOdometryMsg.header.stamp = _timeSurfPointsLessFlat; 305 | _laserOdometryMsg.pose.pose.orientation.x = -geoQuat.y; 306 | _laserOdometryMsg.pose.pose.orientation.y = -geoQuat.z; 307 | _laserOdometryMsg.pose.pose.orientation.z = geoQuat.x; 308 | _laserOdometryMsg.pose.pose.orientation.w = geoQuat.w; 309 | _laserOdometryMsg.pose.pose.position.x = transformSum().pos.x(); 310 | _laserOdometryMsg.pose.pose.position.y = transformSum().pos.y(); 311 | _laserOdometryMsg.pose.pose.position.z = transformSum().pos.z(); 312 | _pubLaserOdometry.publish(_laserOdometryMsg); 313 | 314 | _laserOdometryTrans.stamp_ = _timeSurfPointsLessFlat; 315 | _laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 316 | _laserOdometryTrans.setOrigin(tf::Vector3(transformSum().pos.x(), transformSum().pos.y(), transformSum().pos.z())); 317 | _tfBroadcaster.sendTransform(_laserOdometryTrans); 318 | 319 | // publish cloud results according to the input output ratio 320 | if (_ioRatio < 2 || frameCount() % _ioRatio == 1) 321 | { 322 | ros::Time sweepTime = _timeSurfPointsLessFlat; 323 | publishCloudMsg(_pubLaserCloudCornerLast, *lastCornerCloud(), sweepTime, "/camera"); 324 | publishCloudMsg(_pubLaserCloudSurfLast, *lastSurfaceCloud(), sweepTime, "/camera"); 325 | 326 | transformToEnd(laserCloud()); // transform full resolution cloud to sweep end before sending it 327 | publishCloudMsg(_pubLaserCloudFullRes, *laserCloud(), sweepTime, "/camera"); 328 | } 329 | } 330 | 331 | } // end namespace loam 332 | -------------------------------------------------------------------------------- /src/lib/MultiScanRegistration.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 "loam_velodyne/MultiScanRegistration.h" 34 | #include "math_utils.h" 35 | 36 | #include 37 | 38 | 39 | namespace loam { 40 | 41 | MultiScanMapper::MultiScanMapper(const float& lowerBound, 42 | const float& upperBound, 43 | const uint16_t& nScanRings) 44 | : _lowerBound(lowerBound), 45 | _upperBound(upperBound), 46 | _nScanRings(nScanRings), 47 | _factor((nScanRings - 1) / (upperBound - lowerBound)) 48 | { 49 | 50 | } 51 | 52 | void MultiScanMapper::set(const float &lowerBound, 53 | const float &upperBound, 54 | const uint16_t &nScanRings) 55 | { 56 | _lowerBound = lowerBound; 57 | _upperBound = upperBound; 58 | _nScanRings = nScanRings; 59 | _factor = (nScanRings - 1) / (upperBound - lowerBound); 60 | } 61 | 62 | 63 | 64 | int MultiScanMapper::getRingForAngle(const float& angle) { 65 | return int(((angle * 180 / M_PI) - _lowerBound) * _factor + 0.5); 66 | } 67 | 68 | 69 | 70 | 71 | 72 | 73 | MultiScanRegistration::MultiScanRegistration(const MultiScanMapper& scanMapper) 74 | : _scanMapper(scanMapper) 75 | {}; 76 | 77 | 78 | 79 | bool MultiScanRegistration::setup(ros::NodeHandle& node, ros::NodeHandle& privateNode) 80 | { 81 | RegistrationParams config; 82 | if (!setupROS(node, privateNode, config)) 83 | return false; 84 | 85 | configure(config); 86 | return true; 87 | } 88 | 89 | bool MultiScanRegistration::setupROS(ros::NodeHandle& node, ros::NodeHandle& privateNode, RegistrationParams& config_out) 90 | { 91 | if (!ScanRegistration::setupROS(node, privateNode, config_out)) 92 | return false; 93 | 94 | // fetch scan mapping params 95 | std::string lidarName; 96 | 97 | if (privateNode.getParam("lidar", lidarName)) { 98 | if (lidarName == "VLP-16") { 99 | _scanMapper = MultiScanMapper::Velodyne_VLP_16(); 100 | } else if (lidarName == "HDL-32") { 101 | _scanMapper = MultiScanMapper::Velodyne_HDL_32(); 102 | } else if (lidarName == "HDL-64E") { 103 | _scanMapper = MultiScanMapper::Velodyne_HDL_64E(); 104 | } else { 105 | ROS_ERROR("Invalid lidar parameter: %s (only \"VLP-16\", \"HDL-32\" and \"HDL-64E\" are supported)", lidarName.c_str()); 106 | return false; 107 | } 108 | 109 | ROS_INFO("Set %s scan mapper.", lidarName.c_str()); 110 | if (!privateNode.hasParam("scanPeriod")) { 111 | config_out.scanPeriod = 0.1; 112 | ROS_INFO("Set scanPeriod: %f", config_out.scanPeriod); 113 | } 114 | } else { 115 | float vAngleMin, vAngleMax; 116 | int nScanRings; 117 | 118 | if (privateNode.getParam("minVerticalAngle", vAngleMin) && 119 | privateNode.getParam("maxVerticalAngle", vAngleMax) && 120 | privateNode.getParam("nScanRings", nScanRings)) { 121 | if (vAngleMin >= vAngleMax) { 122 | ROS_ERROR("Invalid vertical range (min >= max)"); 123 | return false; 124 | } else if (nScanRings < 2) { 125 | ROS_ERROR("Invalid number of scan rings (n < 2)"); 126 | return false; 127 | } 128 | 129 | _scanMapper.set(vAngleMin, vAngleMax, nScanRings); 130 | ROS_INFO("Set linear scan mapper from %g to %g degrees with %d scan rings.", vAngleMin, vAngleMax, nScanRings); 131 | } 132 | } 133 | 134 | // subscribe to input cloud topic 135 | _subLaserCloud = node.subscribe 136 | ("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this); 137 | 138 | return true; 139 | } 140 | 141 | 142 | 143 | void MultiScanRegistration::handleCloudMessage(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 144 | { 145 | if (_systemDelay > 0) 146 | { 147 | --_systemDelay; 148 | return; 149 | } 150 | 151 | // fetch new input cloud 152 | pcl::PointCloud laserCloudIn; 153 | pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); 154 | 155 | process(laserCloudIn, fromROSTime(laserCloudMsg->header.stamp)); 156 | } 157 | 158 | 159 | 160 | void MultiScanRegistration::process(const pcl::PointCloud& laserCloudIn, const Time& scanTime) 161 | { 162 | size_t cloudSize = laserCloudIn.size(); 163 | 164 | // determine scan start and end orientations 165 | float startOri = -std::atan2(laserCloudIn[0].y, laserCloudIn[0].x); 166 | float endOri = -std::atan2(laserCloudIn[cloudSize - 1].y, 167 | laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI); 168 | if (endOri - startOri > 3 * M_PI) { 169 | endOri -= 2 * M_PI; 170 | } else if (endOri - startOri < M_PI) { 171 | endOri += 2 * M_PI; 172 | } 173 | 174 | bool halfPassed = false; 175 | pcl::PointXYZI point; 176 | _laserCloudScans.resize(_scanMapper.getNumberOfScanRings()); 177 | // clear all scanline points 178 | std::for_each(_laserCloudScans.begin(), _laserCloudScans.end(), [](auto&&v) {v.clear(); }); 179 | 180 | // extract valid points from input cloud 181 | for (int i = 0; i < cloudSize; i++) { 182 | point.x = laserCloudIn[i].y; 183 | point.y = laserCloudIn[i].z; 184 | point.z = laserCloudIn[i].x; 185 | 186 | // skip NaN and INF valued points 187 | if (!pcl_isfinite(point.x) || 188 | !pcl_isfinite(point.y) || 189 | !pcl_isfinite(point.z)) { 190 | continue; 191 | } 192 | 193 | // skip zero valued points 194 | if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) { 195 | continue; 196 | } 197 | 198 | // calculate vertical point angle and scan ID 199 | float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z)); 200 | int scanID = _scanMapper.getRingForAngle(angle); 201 | if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 ){ 202 | continue; 203 | } 204 | 205 | // calculate horizontal point angle 206 | float ori = -std::atan2(point.x, point.z); 207 | if (!halfPassed) { 208 | if (ori < startOri - M_PI / 2) { 209 | ori += 2 * M_PI; 210 | } else if (ori > startOri + M_PI * 3 / 2) { 211 | ori -= 2 * M_PI; 212 | } 213 | 214 | if (ori - startOri > M_PI) { 215 | halfPassed = true; 216 | } 217 | } else { 218 | ori += 2 * M_PI; 219 | 220 | if (ori < endOri - M_PI * 3 / 2) { 221 | ori += 2 * M_PI; 222 | } else if (ori > endOri + M_PI / 2) { 223 | ori -= 2 * M_PI; 224 | } 225 | } 226 | 227 | // calculate relative scan time based on point orientation 228 | float relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri); 229 | point.intensity = scanID + relTime; 230 | 231 | projectPointToStartOfSweep(point, relTime); 232 | 233 | _laserCloudScans[scanID].push_back(point); 234 | } 235 | 236 | processScanlines(scanTime, _laserCloudScans); 237 | publishResult(); 238 | } 239 | 240 | } // end namespace loam 241 | -------------------------------------------------------------------------------- /src/lib/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 "loam_velodyne/ScanRegistration.h" 34 | #include "math_utils.h" 35 | 36 | #include 37 | 38 | 39 | namespace loam { 40 | 41 | 42 | 43 | bool ScanRegistration::parseParams(const ros::NodeHandle& nh, RegistrationParams& config_out) 44 | { 45 | bool success = true; 46 | int iParam = 0; 47 | float fParam = 0; 48 | 49 | if (nh.getParam("scanPeriod", fParam)) { 50 | if (fParam <= 0) { 51 | ROS_ERROR("Invalid scanPeriod parameter: %f (expected > 0)", fParam); 52 | success = false; 53 | } else { 54 | config_out.scanPeriod = fParam; 55 | ROS_INFO("Set scanPeriod: %g", fParam); 56 | } 57 | } 58 | 59 | if (nh.getParam("imuHistorySize", iParam)) { 60 | if (iParam < 1) { 61 | ROS_ERROR("Invalid imuHistorySize parameter: %d (expected >= 1)", iParam); 62 | success = false; 63 | } else { 64 | config_out.imuHistorySize = iParam; 65 | ROS_INFO("Set imuHistorySize: %d", iParam); 66 | } 67 | } 68 | 69 | if (nh.getParam("featureRegions", iParam)) { 70 | if (iParam < 1) { 71 | ROS_ERROR("Invalid featureRegions parameter: %d (expected >= 1)", iParam); 72 | success = false; 73 | } else { 74 | config_out.nFeatureRegions = iParam; 75 | ROS_INFO("Set nFeatureRegions: %d", iParam); 76 | } 77 | } 78 | 79 | if (nh.getParam("curvatureRegion", iParam)) { 80 | if (iParam < 1) { 81 | ROS_ERROR("Invalid curvatureRegion parameter: %d (expected >= 1)", iParam); 82 | success = false; 83 | } else { 84 | config_out.curvatureRegion = iParam; 85 | ROS_INFO("Set curvatureRegion: +/- %d", iParam); 86 | } 87 | } 88 | 89 | if (nh.getParam("maxCornerSharp", iParam)) { 90 | if (iParam < 1) { 91 | ROS_ERROR("Invalid maxCornerSharp parameter: %d (expected >= 1)", iParam); 92 | success = false; 93 | } else { 94 | config_out.maxCornerSharp = iParam; 95 | config_out.maxCornerLessSharp = 10 * iParam; 96 | ROS_INFO("Set maxCornerSharp / less sharp: %d / %d", iParam, config_out.maxCornerLessSharp); 97 | } 98 | } 99 | 100 | if (nh.getParam("maxCornerLessSharp", iParam)) { 101 | if (iParam < config_out.maxCornerSharp) { 102 | ROS_ERROR("Invalid maxCornerLessSharp parameter: %d (expected >= %d)", iParam, config_out.maxCornerSharp); 103 | success = false; 104 | } else { 105 | config_out.maxCornerLessSharp = iParam; 106 | ROS_INFO("Set maxCornerLessSharp: %d", iParam); 107 | } 108 | } 109 | 110 | if (nh.getParam("maxSurfaceFlat", iParam)) { 111 | if (iParam < 1) { 112 | ROS_ERROR("Invalid maxSurfaceFlat parameter: %d (expected >= 1)", iParam); 113 | success = false; 114 | } else { 115 | config_out.maxSurfaceFlat = iParam; 116 | ROS_INFO("Set maxSurfaceFlat: %d", iParam); 117 | } 118 | } 119 | 120 | if (nh.getParam("surfaceCurvatureThreshold", fParam)) { 121 | if (fParam < 0.001) { 122 | ROS_ERROR("Invalid surfaceCurvatureThreshold parameter: %f (expected >= 0.001)", fParam); 123 | success = false; 124 | } else { 125 | config_out.surfaceCurvatureThreshold = fParam; 126 | ROS_INFO("Set surfaceCurvatureThreshold: %g", fParam); 127 | } 128 | } 129 | 130 | if (nh.getParam("lessFlatFilterSize", fParam)) { 131 | if (fParam < 0.001) { 132 | ROS_ERROR("Invalid lessFlatFilterSize parameter: %f (expected >= 0.001)", fParam); 133 | success = false; 134 | } else { 135 | config_out.lessFlatFilterSize = fParam; 136 | ROS_INFO("Set lessFlatFilterSize: %g", fParam); 137 | } 138 | } 139 | 140 | return success; 141 | } 142 | 143 | bool ScanRegistration::setupROS(ros::NodeHandle& node, ros::NodeHandle& privateNode, RegistrationParams& config_out) 144 | { 145 | if (!parseParams(privateNode, config_out)) 146 | return false; 147 | 148 | // subscribe to IMU topic 149 | _subImu = node.subscribe("/imu/data", 50, &ScanRegistration::handleIMUMessage, this); 150 | 151 | // advertise scan registration topics 152 | _pubLaserCloud = node.advertise("/velodyne_cloud_2", 2); 153 | _pubCornerPointsSharp = node.advertise("/laser_cloud_sharp", 2); 154 | _pubCornerPointsLessSharp = node.advertise("/laser_cloud_less_sharp", 2); 155 | _pubSurfPointsFlat = node.advertise("/laser_cloud_flat", 2); 156 | _pubSurfPointsLessFlat = node.advertise("/laser_cloud_less_flat", 2); 157 | _pubImuTrans = node.advertise("/imu_trans", 5); 158 | 159 | return true; 160 | } 161 | 162 | 163 | 164 | void ScanRegistration::handleIMUMessage(const sensor_msgs::Imu::ConstPtr& imuIn) 165 | { 166 | tf::Quaternion orientation; 167 | tf::quaternionMsgToTF(imuIn->orientation, orientation); 168 | double roll, pitch, yaw; 169 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); 170 | 171 | Vector3 acc; 172 | acc.x() = float(imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81); 173 | acc.y() = float(imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81); 174 | acc.z() = float(imuIn->linear_acceleration.x + sin(pitch) * 9.81); 175 | 176 | IMUState newState; 177 | newState.stamp = fromROSTime( imuIn->header.stamp); 178 | newState.roll = roll; 179 | newState.pitch = pitch; 180 | newState.yaw = yaw; 181 | newState.acceleration = acc; 182 | 183 | updateIMUData(acc, newState); 184 | } 185 | 186 | 187 | void ScanRegistration::publishResult() 188 | { 189 | auto sweepStartTime = toROSTime(sweepStart()); 190 | // publish full resolution and feature point clouds 191 | publishCloudMsg(_pubLaserCloud, laserCloud(), sweepStartTime, "/camera"); 192 | publishCloudMsg(_pubCornerPointsSharp, cornerPointsSharp(), sweepStartTime, "/camera"); 193 | publishCloudMsg(_pubCornerPointsLessSharp, cornerPointsLessSharp(), sweepStartTime, "/camera"); 194 | publishCloudMsg(_pubSurfPointsFlat, surfacePointsFlat(), sweepStartTime, "/camera"); 195 | publishCloudMsg(_pubSurfPointsLessFlat, surfacePointsLessFlat(), sweepStartTime, "/camera"); 196 | 197 | // publish corresponding IMU transformation information 198 | publishCloudMsg(_pubImuTrans, imuTransform(), sweepStartTime, "/camera"); 199 | } 200 | 201 | } // end namespace loam 202 | -------------------------------------------------------------------------------- /src/lib/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 "loam_velodyne/TransformMaintenance.h" 34 | 35 | namespace loam 36 | { 37 | 38 | TransformMaintenance::TransformMaintenance() 39 | { 40 | // initialize odometry and odometry tf messages 41 | _laserOdometry2.header.frame_id = "/camera_init"; 42 | _laserOdometry2.child_frame_id = "/camera"; 43 | 44 | _laserOdometryTrans2.frame_id_ = "/camera_init"; 45 | _laserOdometryTrans2.child_frame_id_ = "/camera"; 46 | } 47 | 48 | 49 | bool TransformMaintenance::setup(ros::NodeHandle &node, ros::NodeHandle &privateNode) 50 | { 51 | // advertise integrated laser odometry topic 52 | _pubLaserOdometry2 = node.advertise("/integrated_to_init", 5); 53 | 54 | // subscribe to laser odometry and mapping odometry topics 55 | _subLaserOdometry = node.subscribe 56 | ("/laser_odom_to_init", 5, &TransformMaintenance::laserOdometryHandler, this); 57 | 58 | _subOdomAftMapped = node.subscribe 59 | ("/aft_mapped_to_init", 5, &TransformMaintenance::odomAftMappedHandler, this); 60 | 61 | return true; 62 | } 63 | 64 | 65 | 66 | void TransformMaintenance::laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) 67 | { 68 | double roll, pitch, yaw; 69 | geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation; 70 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 71 | 72 | updateOdometry(-pitch, -yaw, roll, 73 | laserOdometry->pose.pose.position.x, 74 | laserOdometry->pose.pose.position.y, 75 | laserOdometry->pose.pose.position.z); 76 | 77 | transformAssociateToMap(); 78 | 79 | geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformMapped()[2], -transformMapped()[0], -transformMapped()[1]); 80 | 81 | _laserOdometry2.header.stamp = laserOdometry->header.stamp; 82 | _laserOdometry2.pose.pose.orientation.x = -geoQuat.y; 83 | _laserOdometry2.pose.pose.orientation.y = -geoQuat.z; 84 | _laserOdometry2.pose.pose.orientation.z = geoQuat.x; 85 | _laserOdometry2.pose.pose.orientation.w = geoQuat.w; 86 | _laserOdometry2.pose.pose.position.x = transformMapped()[3]; 87 | _laserOdometry2.pose.pose.position.y = transformMapped()[4]; 88 | _laserOdometry2.pose.pose.position.z = transformMapped()[5]; 89 | _pubLaserOdometry2.publish(_laserOdometry2); 90 | 91 | _laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; 92 | _laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 93 | _laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped()[3], transformMapped()[4], transformMapped()[5])); 94 | _tfBroadcaster2.sendTransform(_laserOdometryTrans2); 95 | } 96 | 97 | void TransformMaintenance::odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped) 98 | { 99 | double roll, pitch, yaw; 100 | geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation; 101 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 102 | 103 | updateMappingTransform(-pitch, -yaw, roll, 104 | odomAftMapped->pose.pose.position.x, 105 | odomAftMapped->pose.pose.position.y, 106 | odomAftMapped->pose.pose.position.z, 107 | 108 | odomAftMapped->twist.twist.angular.x, 109 | odomAftMapped->twist.twist.angular.y, 110 | odomAftMapped->twist.twist.angular.z, 111 | 112 | odomAftMapped->twist.twist.linear.x, 113 | odomAftMapped->twist.twist.linear.y, 114 | odomAftMapped->twist.twist.linear.z); 115 | } 116 | 117 | } // end namespace loam 118 | -------------------------------------------------------------------------------- /src/lib/math_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef LOAM_MATH_UTILS_H 2 | #define LOAM_MATH_UTILS_H 3 | 4 | 5 | #include "loam_velodyne/Angle.h" 6 | #include "loam_velodyne/Vector3.h" 7 | 8 | #include 9 | 10 | 11 | namespace loam { 12 | 13 | /** \brief Convert the given radian angle to degrees. 14 | * 15 | * @param radians The radian angle to convert. 16 | * @return The angle in degrees. 17 | */ 18 | inline double rad2deg(double radians) 19 | { 20 | return radians * 180.0 / M_PI; 21 | } 22 | 23 | 24 | 25 | /** \brief Convert the given radian angle to degrees. 26 | * 27 | * @param radians The radian angle to convert. 28 | * @return The angle in degrees. 29 | */ 30 | inline float rad2deg(float radians) 31 | { 32 | return (float) (radians * 180.0 / M_PI); 33 | } 34 | 35 | 36 | 37 | /** \brief Convert the given degree angle to radian. 38 | * 39 | * @param degrees The degree angle to convert. 40 | * @return The radian angle. 41 | */ 42 | inline double deg2rad(double degrees) 43 | { 44 | return degrees * M_PI / 180.0; 45 | } 46 | 47 | 48 | 49 | /** \brief Convert the given degree angle to radian. 50 | * 51 | * @param degrees The degree angle to convert. 52 | * @return The radian angle. 53 | */ 54 | inline float deg2rad(float degrees) 55 | { 56 | return (float) (degrees * M_PI / 180.0); 57 | } 58 | 59 | 60 | 61 | 62 | /** \brief Calculate the squared difference of the given two points. 63 | * 64 | * @param a The first point. 65 | * @param b The second point. 66 | * @return The squared difference between point a and b. 67 | */ 68 | template 69 | inline float calcSquaredDiff(const PointT& a, const PointT& b) 70 | { 71 | float diffX = a.x - b.x; 72 | float diffY = a.y - b.y; 73 | float diffZ = a.z - b.z; 74 | 75 | return diffX * diffX + diffY * diffY + diffZ * diffZ; 76 | } 77 | 78 | 79 | 80 | /** \brief Calculate the squared difference of the given two points. 81 | * 82 | * @param a The first point. 83 | * @param b The second point. 84 | * @param wb The weighting factor for the SECOND point. 85 | * @return The squared difference between point a and b. 86 | */ 87 | template 88 | inline float calcSquaredDiff(const PointT& a, const PointT& b, const float& wb) 89 | { 90 | float diffX = a.x - b.x * wb; 91 | float diffY = a.y - b.y * wb; 92 | float diffZ = a.z - b.z * wb; 93 | 94 | return diffX * diffX + diffY * diffY + diffZ * diffZ; 95 | } 96 | 97 | 98 | /** \brief Calculate the absolute distance of the point to the origin. 99 | * 100 | * @param p The point. 101 | * @return The distance to the point. 102 | */ 103 | template 104 | inline float calcPointDistance(const PointT& p) 105 | { 106 | return std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z); 107 | } 108 | 109 | 110 | 111 | /** \brief Calculate the squared distance of the point to the origin. 112 | * 113 | * @param p The point. 114 | * @return The squared distance to the point. 115 | */ 116 | template 117 | inline float calcSquaredPointDistance(const PointT& p) 118 | { 119 | return p.x * p.x + p.y * p.y + p.z * p.z; 120 | } 121 | 122 | 123 | 124 | /** \brief Rotate the given vector by the specified angle around the x-axis. 125 | * 126 | * @param v the vector to rotate 127 | * @param ang the rotation angle 128 | */ 129 | inline void rotX(Vector3& v, const Angle& ang) 130 | { 131 | float y = v.y(); 132 | v.y() = ang.cos() * y - ang.sin() * v.z(); 133 | v.z() = ang.sin() * y + ang.cos() * v.z(); 134 | } 135 | 136 | /** \brief Rotate the given point by the specified angle around the x-axis. 137 | * 138 | * @param p the point to rotate 139 | * @param ang the rotation angle 140 | */ 141 | template 142 | inline void rotX(PointT& p, const Angle& ang) 143 | { 144 | float y = p.y; 145 | p.y = ang.cos() * y - ang.sin() * p.z; 146 | p.z = ang.sin() * y + ang.cos() * p.z; 147 | } 148 | 149 | 150 | 151 | /** \brief Rotate the given vector by the specified angle around the y-axis. 152 | * 153 | * @param v the vector to rotate 154 | * @param ang the rotation angle 155 | */ 156 | inline void rotY(Vector3& v, const Angle& ang) 157 | { 158 | float x = v.x(); 159 | v.x() = ang.cos() * x + ang.sin() * v.z(); 160 | v.z() = ang.cos() * v.z() - ang.sin() * x; 161 | } 162 | 163 | /** \brief Rotate the given point by the specified angle around the y-axis. 164 | * 165 | * @param p the point to rotate 166 | * @param ang the rotation angle 167 | */ 168 | template 169 | inline void rotY(PointT& p, const Angle& ang) 170 | { 171 | float x = p.x; 172 | p.x = ang.cos() * x + ang.sin() * p.z; 173 | p.z = ang.cos() * p.z - ang.sin() * x; 174 | } 175 | 176 | 177 | 178 | /** \brief Rotate the given vector by the specified angle around the z-axis. 179 | * 180 | * @param v the vector to rotate 181 | * @param ang the rotation angle 182 | */ 183 | inline void rotZ(Vector3& v, const Angle& ang) 184 | { 185 | float x = v.x(); 186 | v.x() = ang.cos() * x - ang.sin() * v.y(); 187 | v.y() = ang.sin() * x + ang.cos() * v.y(); 188 | } 189 | 190 | /** \brief Rotate the given point by the specified angle around the z-axis. 191 | * 192 | * @param p the point to rotate 193 | * @param ang the rotation angle 194 | */ 195 | template 196 | inline void rotZ(PointT& p, const Angle& ang) 197 | { 198 | float x = p.x; 199 | p.x = ang.cos() * x - ang.sin() * p.y; 200 | p.y = ang.sin() * x + ang.cos() * p.y; 201 | } 202 | 203 | 204 | 205 | /** \brief Rotate the given vector by the specified angles around the z-, x- respectively y-axis. 206 | * 207 | * @param v the vector to rotate 208 | * @param angZ the rotation angle around the z-axis 209 | * @param angX the rotation angle around the x-axis 210 | * @param angY the rotation angle around the y-axis 211 | */ 212 | inline void rotateZXY(Vector3& v, 213 | const Angle& angZ, 214 | const Angle& angX, 215 | const Angle& angY) 216 | { 217 | rotZ(v, angZ); 218 | rotX(v, angX); 219 | rotY(v, angY); 220 | } 221 | 222 | /** \brief Rotate the given point by the specified angles around the z-, x- respectively y-axis. 223 | * 224 | * @param p the point to rotate 225 | * @param angZ the rotation angle around the z-axis 226 | * @param angX the rotation angle around the x-axis 227 | * @param angY the rotation angle around the y-axis 228 | */ 229 | template 230 | inline void rotateZXY(PointT& p, 231 | const Angle& angZ, 232 | const Angle& angX, 233 | const Angle& angY) 234 | { 235 | rotZ(p, angZ); 236 | rotX(p, angX); 237 | rotY(p, angY); 238 | } 239 | 240 | 241 | 242 | /** \brief Rotate the given vector by the specified angles around the y-, x- respectively z-axis. 243 | * 244 | * @param v the vector to rotate 245 | * @param angY the rotation angle around the y-axis 246 | * @param angX the rotation angle around the x-axis 247 | * @param angZ the rotation angle around the z-axis 248 | */ 249 | inline void rotateYXZ(Vector3& v, 250 | const Angle& angY, 251 | const Angle& angX, 252 | const Angle& angZ) 253 | { 254 | rotY(v, angY); 255 | rotX(v, angX); 256 | rotZ(v, angZ); 257 | } 258 | 259 | /** \brief Rotate the given point by the specified angles around the y-, x- respectively z-axis. 260 | * 261 | * @param p the point to rotate 262 | * @param angY the rotation angle around the y-axis 263 | * @param angX the rotation angle around the x-axis 264 | * @param angZ the rotation angle around the z-axis 265 | */ 266 | template 267 | inline void rotateYXZ(PointT& p, 268 | const Angle& angY, 269 | const Angle& angX, 270 | const Angle& angZ) 271 | { 272 | rotY(p, angY); 273 | rotX(p, angX); 274 | rotZ(p, angZ); 275 | } 276 | 277 | } // end namespace loam 278 | 279 | 280 | #endif // LOAM_MATH_UTILS_H 281 | -------------------------------------------------------------------------------- /src/multi_scan_registration_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "loam_velodyne/MultiScanRegistration.h" 3 | 4 | 5 | /** Main node entry point. */ 6 | int main(int argc, char **argv) 7 | { 8 | ros::init(argc, argv, "scanRegistration"); 9 | ros::NodeHandle node; 10 | ros::NodeHandle privateNode("~"); 11 | 12 | loam::MultiScanRegistration multiScan; 13 | 14 | if (multiScan.setup(node, privateNode)) { 15 | // initialization successful 16 | ros::spin(); 17 | } 18 | 19 | return 0; 20 | } 21 | -------------------------------------------------------------------------------- /src/transform_maintenance_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "loam_velodyne/TransformMaintenance.h" 3 | 4 | 5 | /** Main node entry point. */ 6 | int main(int argc, char **argv) 7 | { 8 | ros::init(argc, argv, "transformMaintenance"); 9 | ros::NodeHandle node; 10 | ros::NodeHandle privateNode("~"); 11 | 12 | loam::TransformMaintenance transMaintenance; 13 | 14 | if (transMaintenance.setup(node, privateNode)) { 15 | // initialization successful 16 | ros::spin(); 17 | } 18 | 19 | return 0; 20 | } 21 | -------------------------------------------------------------------------------- /tests/bag_test: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import datetime 4 | import os 5 | import rosbag 6 | import rostest 7 | import sys 8 | import time 9 | import unittest 10 | import sensor_msgs.point_cloud2 as point_cloud2 11 | from itertools import izip 12 | 13 | ''' 14 | A simple test to play test data into the system and record the results, then 15 | verify the results. 16 | Usage 17 | 18 | bag_test 19 | ''' 20 | 21 | class TestBag(unittest.TestCase): 22 | def test_bags_match(self): 23 | # Load comparison bag 24 | comparison = dict() 25 | for topic, msg, t in rosbag.Bag(sys.argv[1]).read_messages(): 26 | # This also replaces tf timestamps under the assumption 27 | # that all transforms in the message share the same timestamp 28 | if msg._type == 'tf2_msgs/TFMessage': 29 | for m in msg.transforms: 30 | comparison[(topic, m.header.stamp, m.header.frame_id, m.child_frame_id)] = m 31 | else: 32 | try: 33 | comparison[(topic, msg.header.stamp.to_sec())] = msg 34 | except: 35 | pass 36 | # Wait for DUT to finish filling up the test bag (30 sec) 37 | time.sleep(35.0-(datetime.datetime.today()-start_time).seconds) 38 | test_output_bag = sys.argv[2] 39 | while not os.path.isfile(test_output_bag): 40 | time.sleep(0.2) 41 | # Check that all messages are the same 42 | for topic, msg, t in rosbag.Bag(test_output_bag).read_messages(): 43 | if msg._type == 'sensor_msgs/PointCloud2': 44 | if (topic, msg.header.stamp.to_sec()) in comparison: 45 | comparison_msg = comparison[(topic, msg.header.stamp.to_sec())] 46 | for datum1, datum2 in izip(point_cloud2.read_points(comparison_msg), point_cloud2.read_points(msg)): 47 | self.assertEquals(datum1, datum2, "{} != {}".format(datum1, datum2)) 48 | os.remove(test_output_bag) 49 | 50 | if __name__ == '__main__': 51 | global start_time 52 | start_time = datetime.datetime.today() 53 | rostest.rosrun('loam_velodyne', 'test_bag', TestBag) 54 | -------------------------------------------------------------------------------- /tests/loam.test.in: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | --------------------------------------------------------------------------------