├── .clangformat ├── LICENSE ├── LeGO-LOAM ├── CMakeLists.txt ├── config │ └── loam_config.yaml ├── include │ └── lego_loam │ │ ├── channel.h │ │ ├── nanoflann.hpp │ │ ├── nanoflann_pcl.h │ │ └── utility.h ├── launch │ ├── block.png │ ├── dataset-demo.gif │ ├── demo.gif │ ├── google-earth.png │ ├── jackal-label.jpg │ ├── odometry.jpg │ ├── run.launch │ ├── seg-total.jpg │ └── test.rviz ├── package.xml └── src │ ├── featureAssociation.cpp │ ├── featureAssociation.h │ ├── imageProjection.cpp │ ├── imageProjection.h │ ├── main.cpp │ ├── mapOptimization.h │ ├── mapOptmization.cpp │ ├── transformFusion.cpp │ └── transformFusion.h ├── README.md ├── Shan_Englot_IROS_2018_Preprint.pdf └── cloud_msgs ├── CMakeLists.txt ├── msg └── cloud_info.msg └── package.xml /.clangformat: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | ConstructorInitializerIndentWidth: 2 5 | AlignEscapedNewlinesLeft: false 6 | AlignTrailingComments: true 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortFunctionsOnASingleLine: None 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakTemplateDeclarations: true 13 | AlwaysBreakBeforeMultilineStrings: false 14 | BreakBeforeBinaryOperators: false 15 | BreakBeforeTernaryOperators: false 16 | BreakConstructorInitializersBeforeComma: true 17 | BinPackParameters: true 18 | ColumnLimit: 120 19 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 20 | DerivePointerBinding: false 21 | PointerBindsToType: true 22 | ExperimentalAutoDetectBinPacking: false 23 | IndentCaseLabels: true 24 | MaxEmptyLinesToKeep: 1 25 | NamespaceIndentation: None 26 | ObjCSpaceBeforeProtocolList: true 27 | PenaltyBreakBeforeFirstCallParameter: 19 28 | PenaltyBreakComment: 60 29 | PenaltyBreakString: 1 30 | PenaltyBreakFirstLessLess: 1000 31 | PenaltyExcessCharacter: 1000 32 | PenaltyReturnTypeOnItsOwnLine: 90 33 | SpacesBeforeTrailingComments: 2 34 | Cpp11BracedListStyle: false 35 | Standard: Auto 36 | IndentWidth: 2 37 | TabWidth: 2 38 | UseTab: Never 39 | IndentFunctionDeclarationAfterType: false 40 | SpacesInParentheses: false 41 | SpacesInAngles: false 42 | SpaceInEmptyParentheses: false 43 | SpacesInCStyleCastParentheses: false 44 | SpaceAfterControlStatementKeyword: true 45 | SpaceBeforeAssignmentOperators: true 46 | ContinuationIndentWidth: 4 47 | SortIncludes: false 48 | SpaceAfterCStyleCast: false 49 | 50 | # Configure each individual brace in BraceWrapping 51 | BreakBeforeBraces: Custom 52 | 53 | # Control of individual brace wrapping cases 54 | BraceWrapping: { 55 | AfterClass: 'true' 56 | AfterControlStatement: 'true' 57 | AfterEnum : 'true' 58 | AfterFunction : 'true' 59 | AfterNamespace : 'true' 60 | AfterStruct : 'true' 61 | AfterUnion : 'true' 62 | BeforeCatch : 'true' 63 | BeforeElse : 'true' 64 | IndentBraces : 'false' 65 | } 66 | ... 67 | 68 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Robust Field Autonomy Lab 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /LeGO-LOAM/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lego_loam_bor) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -g ") 5 | 6 | # 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | tf 10 | roscpp 11 | rospy 12 | image_transport 13 | rosbag_storage 14 | 15 | pcl_ros 16 | pcl_conversions 17 | 18 | std_msgs 19 | sensor_msgs 20 | geometry_msgs 21 | nav_msgs 22 | cloud_msgs 23 | ) 24 | 25 | find_package(GTSAM REQUIRED QUIET) 26 | find_package(PCL REQUIRED QUIET) 27 | 28 | catkin_package( 29 | INCLUDE_DIRS include 30 | CATKIN_DEPENDS cloud_msgs rosbag_storage 31 | DEPENDS PCL 32 | ) 33 | 34 | include_directories( 35 | include 36 | ${catkin_INCLUDE_DIRS} 37 | ${PCL_INCLUDE_DIRS} 38 | ${GTSAM_INCLUDE_DIR} 39 | ) 40 | 41 | link_directories( 42 | include 43 | ${PCL_LIBRARY_DIRS} 44 | ${GTSAM_LIBRARY_DIRS} 45 | ) 46 | 47 | add_executable(lego_loam_bor 48 | src/imageProjection.cpp 49 | src/featureAssociation.cpp 50 | src/mapOptmization.cpp 51 | src/transformFusion.cpp 52 | src/main.cpp) 53 | 54 | add_dependencies(lego_loam_bor ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp) 55 | target_link_libraries(lego_loam_bor ${catkin_LIBRARIES} ${PCL_LIBRARIES} gtsam) 56 | 57 | -------------------------------------------------------------------------------- /LeGO-LOAM/config/loam_config.yaml: -------------------------------------------------------------------------------- 1 | lego_loam: 2 | 3 | # VLP-16 4 | laser: 5 | num_vertical_scans: 16 6 | num_horizontal_scans: 1800 7 | ground_scan_index: 7 8 | vertical_angle_bottom: -15 # degrees 9 | vertical_angle_top: 15 # degrees 10 | sensor_mount_angle: 0 # degrees 11 | scan_period: 0.1 # seconds 12 | 13 | imageProjection: 14 | segment_valid_point_num: 5 15 | segment_valid_line_num: 3 16 | segment_theta: 60.0 # decrese this value may improve accuracy 17 | 18 | featureAssociation: 19 | edge_threshold: 0.1 20 | surf_threshold: 0.1 21 | nearest_feature_search_distance: 5 22 | 23 | mapping: 24 | enable_loop_closure: false 25 | mapping_frequency_divider: 5 26 | 27 | surrounding_keyframe_search_radius: 50.0 # key frame that is within n meters from current pose will be 28 | # considered for scan-to-map optimization (when loop closure disabled) 29 | surrounding_keyframe_search_num: 50 # submap size (when loop closure enabled) 30 | 31 | history_keyframe_search_radius: 7.0 # key frame that is within n meters from current pose will be considerd for loop closure 32 | history_keyframe_search_num: 25 # 2n+1 number of history key frames will be fused into a submap for loop closure 33 | history_keyframe_fitness_score: 0.3 # the smaller the better alignment 34 | 35 | global_map_visualization_search_radius: 500.0 # key frames with in n meters will be visualized 36 | -------------------------------------------------------------------------------- /LeGO-LOAM/include/lego_loam/channel.h: -------------------------------------------------------------------------------- 1 | #ifndef CHANNEL_H 2 | #define CHANNEL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | // Simplistic mechanism to move information from one thread to another in a thread-safe way. 9 | // Sender will wake up the receiver. 10 | // This will work only for 1 writer and 1 reader. Not buffered. 11 | template class Channel { 12 | private: 13 | T _item; 14 | bool _empty; 15 | bool _blocking_send; 16 | std::mutex _m; 17 | std::condition_variable _cv; 18 | public: 19 | 20 | Channel(bool blocking_send ): _empty(true), _blocking_send(blocking_send) {} 21 | 22 | // Move an item into the channel. 23 | // Block if not empty 24 | void send(T&& item) { 25 | std::unique_lock lock(_m); 26 | if(_blocking_send){ 27 | _cv.wait(lock, [&](){ return _empty; }); 28 | } 29 | _item = std::move(item); 30 | _empty = false; 31 | _cv.notify_all(); 32 | } 33 | 34 | // Copy an item into the channel. 35 | // Block if not empty 36 | void send(const T &item) { 37 | std::unique_lock lock(_m); 38 | if(_blocking_send){ 39 | _cv.wait(lock, [&](){ return _empty; }); 40 | } 41 | _item = item; 42 | _empty = false; 43 | _cv.notify_all(); 44 | } 45 | 46 | // Move an item out of the channel. 47 | // Block if empty 48 | void receive(T &item) { 49 | std::unique_lock lock(_m); 50 | _cv.wait(lock, [&](){ return !_empty; }); 51 | item = std::move(_item); 52 | _empty = true; 53 | _cv.notify_all(); 54 | } 55 | 56 | }; 57 | 58 | #endif // CHANNEL_H 59 | -------------------------------------------------------------------------------- /LeGO-LOAM/include/lego_loam/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 155 | inline int KdTreeFLANN::radiusSearch( 156 | const PointT &point, double radius, std::vector &k_indices, 157 | std::vector &k_sqr_distances) const 158 | { 159 | std::vector> indices_dist; 160 | indices_dist.reserve(128); 161 | RadiusResultSet resultSet(static_cast(radius * radius), 162 | indices_dist); 163 | _kdtree.findNeighbors(resultSet, point.data, _params); 164 | const size_t nFound = resultSet.size(); 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 < nFound; i++) { 171 | k_indices[i] = indices_dist[i].first; 172 | k_sqr_distances[i] = indices_dist[i].second; 173 | } 174 | return nFound; 175 | } 176 | 177 | template 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 | -------------------------------------------------------------------------------- /LeGO-LOAM/include/lego_loam/utility.h: -------------------------------------------------------------------------------- 1 | #ifndef _UTILITY_LIDAR_ODOMETRY_H_ 2 | #define _UTILITY_LIDAR_ODOMETRY_H_ 3 | 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "cloud_msgs/cloud_info.h" 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | typedef pcl::PointXYZI PointType; 45 | 46 | typedef Eigen::Vector3f Vector3; 47 | 48 | const double DEG_TO_RAD = M_PI / 180.0; 49 | 50 | 51 | struct smoothness_t{ 52 | float value; 53 | size_t ind; 54 | }; 55 | 56 | struct by_value{ 57 | bool operator()(smoothness_t const &left, smoothness_t const &right) { 58 | return left.value < right.value; 59 | } 60 | }; 61 | 62 | struct ProjectionOut 63 | { 64 | pcl::PointCloud::Ptr segmented_cloud; 65 | pcl::PointCloud::Ptr outlier_cloud; 66 | cloud_msgs::cloud_info seg_msg; 67 | }; 68 | 69 | 70 | struct AssociationOut 71 | { 72 | pcl::PointCloud::Ptr cloud_outlier_last; 73 | pcl::PointCloud::Ptr cloud_corner_last; 74 | pcl::PointCloud::Ptr cloud_surf_last; 75 | nav_msgs::Odometry laser_odometry; 76 | }; 77 | 78 | struct RollPitchYaw{ 79 | double roll; 80 | double pitch; 81 | double yaw; 82 | RollPitchYaw():roll(0),pitch(0),yaw(0) {} 83 | }; 84 | 85 | struct Transform 86 | { 87 | Transform():pos(Vector3::Zero()) {} 88 | Vector3 pos; 89 | RollPitchYaw rot; 90 | }; 91 | 92 | inline void OdometryToTransform(const nav_msgs::Odometry& odometry, 93 | float* transform) { 94 | double roll, pitch, yaw; 95 | geometry_msgs::Quaternion geoQuat = odometry.pose.pose.orientation; 96 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)) 97 | .getRPY(roll, pitch, yaw); 98 | 99 | transform[0] = -pitch; 100 | transform[1] = -yaw; 101 | transform[2] = roll; 102 | 103 | transform[3] = odometry.pose.pose.position.x; 104 | transform[4] = odometry.pose.pose.position.y; 105 | transform[5] = odometry.pose.pose.position.z; 106 | } 107 | 108 | /* 109 | * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp) 110 | */ 111 | struct PointXYZIRPYT 112 | { 113 | PCL_ADD_POINT4D 114 | PCL_ADD_INTENSITY; 115 | float roll; 116 | float pitch; 117 | float yaw; 118 | double time; 119 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 120 | } EIGEN_ALIGN16; 121 | 122 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT, 123 | (float, x, x) (float, y, y) 124 | (float, z, z) (float, intensity, intensity) 125 | (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw) 126 | (double, time, time) 127 | ) 128 | 129 | typedef PointXYZIRPYT PointTypePose; 130 | 131 | #endif 132 | -------------------------------------------------------------------------------- /LeGO-LOAM/launch/block.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facontidavide/LeGO-LOAM-BOR/5fbb287e3817ba315d4012a22561cf2c88164fd5/LeGO-LOAM/launch/block.png -------------------------------------------------------------------------------- /LeGO-LOAM/launch/dataset-demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facontidavide/LeGO-LOAM-BOR/5fbb287e3817ba315d4012a22561cf2c88164fd5/LeGO-LOAM/launch/dataset-demo.gif -------------------------------------------------------------------------------- /LeGO-LOAM/launch/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facontidavide/LeGO-LOAM-BOR/5fbb287e3817ba315d4012a22561cf2c88164fd5/LeGO-LOAM/launch/demo.gif -------------------------------------------------------------------------------- /LeGO-LOAM/launch/google-earth.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facontidavide/LeGO-LOAM-BOR/5fbb287e3817ba315d4012a22561cf2c88164fd5/LeGO-LOAM/launch/google-earth.png -------------------------------------------------------------------------------- /LeGO-LOAM/launch/jackal-label.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facontidavide/LeGO-LOAM-BOR/5fbb287e3817ba315d4012a22561cf2c88164fd5/LeGO-LOAM/launch/jackal-label.jpg -------------------------------------------------------------------------------- /LeGO-LOAM/launch/odometry.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facontidavide/LeGO-LOAM-BOR/5fbb287e3817ba315d4012a22561cf2c88164fd5/LeGO-LOAM/launch/odometry.jpg -------------------------------------------------------------------------------- /LeGO-LOAM/launch/run.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 | -------------------------------------------------------------------------------- /LeGO-LOAM/launch/seg-total.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facontidavide/LeGO-LOAM-BOR/5fbb287e3817ba315d4012a22561cf2c88164fd5/LeGO-LOAM/launch/seg-total.jpg -------------------------------------------------------------------------------- /LeGO-LOAM/launch/test.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Map Cloud1 8 | - /Map Cloud1/Autocompute Value Bounds1 9 | - /Odometry1 10 | - /Odometry1/Shape1 11 | Splitter Ratio: 0.641666651 12 | Tree Height: 694 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: ~ 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679016 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: Edge Features (green) 29 | - Class: rviz/Help 30 | Name: Help 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Class: rviz/Axes 37 | Enabled: true 38 | Length: 1 39 | Name: base_link 40 | Radius: 0.300000012 41 | Reference Frame: base_link 42 | Value: true 43 | - Alpha: 1 44 | Autocompute Intensity Bounds: true 45 | Autocompute Value Bounds: 46 | Max Value: 20.4566002 47 | Min Value: -3.58307004 48 | Value: true 49 | Axis: Z 50 | Channel Name: intensity 51 | Class: rviz/PointCloud2 52 | Color: 0; 255; 0 53 | Color Transformer: FlatColor 54 | Decay Time: 0 55 | Enabled: true 56 | Invert Rainbow: false 57 | Max Color: 255; 255; 255 58 | Max Intensity: 96 59 | Min Color: 0; 0; 0 60 | Min Intensity: 1 61 | Name: Edge Features (green) 62 | Position Transformer: XYZ 63 | Queue Size: 2 64 | Selectable: true 65 | Size (Pixels): 5 66 | Size (m): 0.00999999978 67 | Style: Points 68 | Topic: /laser_cloud_less_sharp 69 | Unreliable: false 70 | Use Fixed Frame: true 71 | Use rainbow: true 72 | Value: true 73 | - Alpha: 1 74 | Autocompute Intensity Bounds: true 75 | Autocompute Value Bounds: 76 | Max Value: 20.4566002 77 | Min Value: -3.58307004 78 | Value: true 79 | Axis: Z 80 | Channel Name: intensity 81 | Class: rviz/PointCloud2 82 | Color: 255; 170; 255 83 | Color Transformer: FlatColor 84 | Decay Time: 0 85 | Enabled: true 86 | Invert Rainbow: false 87 | Max Color: 255; 255; 255 88 | Max Intensity: 96 89 | Min Color: 0; 0; 0 90 | Min Intensity: 1 91 | Name: Surface Features (pink) 92 | Position Transformer: XYZ 93 | Queue Size: 2 94 | Selectable: true 95 | Size (Pixels): 3 96 | Size (m): 0.00999999978 97 | Style: Points 98 | Topic: /laser_cloud_less_flat 99 | Unreliable: false 100 | Use Fixed Frame: true 101 | Use rainbow: true 102 | Value: true 103 | - Alpha: 1 104 | Autocompute Intensity Bounds: true 105 | Autocompute Value Bounds: 106 | Max Value: 51.7289886 107 | Min Value: -12.3394938 108 | Value: true 109 | Axis: Y 110 | Channel Name: intensity 111 | Class: rviz/PointCloud2 112 | Color: 255; 255; 255 113 | Color Transformer: AxisColor 114 | Decay Time: 0 115 | Enabled: true 116 | Invert Rainbow: false 117 | Max Color: 255; 255; 255 118 | Max Intensity: 15 119 | Min Color: 0; 0; 0 120 | Min Intensity: 0 121 | Name: Map Cloud 122 | Position Transformer: XYZ 123 | Queue Size: 10 124 | Selectable: true 125 | Size (Pixels): 1 126 | Size (m): 0.00999999978 127 | Style: Points 128 | Topic: /laser_cloud_surround 129 | Unreliable: false 130 | Use Fixed Frame: false 131 | Use rainbow: false 132 | Value: true 133 | - Alpha: 1 134 | Autocompute Intensity Bounds: true 135 | Autocompute Value Bounds: 136 | Max Value: 4.57089996 137 | Min Value: 0.300000012 138 | Value: true 139 | Axis: Z 140 | Channel Name: intensity 141 | Class: rviz/PointCloud2 142 | Color: 255; 255; 255 143 | Color Transformer: Intensity 144 | Decay Time: 0 145 | Enabled: true 146 | Invert Rainbow: false 147 | Max Color: 255; 255; 255 148 | Max Intensity: 113 149 | Min Color: 0; 0; 0 150 | Min Intensity: 0 151 | Name: Trajectory 152 | Position Transformer: XYZ 153 | Queue Size: 1 154 | Selectable: true 155 | Size (Pixels): 3 156 | Size (m): 0.200000003 157 | Style: Flat Squares 158 | Topic: /key_pose_origin 159 | Unreliable: false 160 | Use Fixed Frame: true 161 | Use rainbow: true 162 | Value: true 163 | - Angle Tolerance: 0.200000003 164 | Class: rviz/Odometry 165 | Covariance: 166 | Orientation: 167 | Alpha: 0.5 168 | Color: 255; 255; 127 169 | Color Style: Unique 170 | Frame: Local 171 | Offset: 1 172 | Scale: 1 173 | Value: true 174 | Position: 175 | Alpha: 0.300000012 176 | Color: 204; 51; 204 177 | Scale: 1 178 | Value: true 179 | Value: true 180 | Enabled: true 181 | Keep: 100000 182 | Name: Odometry 183 | Position Tolerance: 0.300000012 184 | Shape: 185 | Alpha: 1 186 | Axes Length: 1 187 | Axes Radius: 0.100000001 188 | Color: 255; 25; 0 189 | Head Length: 0.800000012 190 | Head Radius: 0.5 191 | Shaft Length: 1 192 | Shaft Radius: 0.200000003 193 | Value: Arrow 194 | Topic: /aft_mapped_to_init 195 | Unreliable: false 196 | Value: true 197 | Enabled: true 198 | Global Options: 199 | Background Color: 0; 0; 0 200 | Default Light: true 201 | Fixed Frame: map 202 | Frame Rate: 30 203 | Name: root 204 | Tools: 205 | - Class: rviz/Interact 206 | Hide Inactive Objects: true 207 | - Class: rviz/FocusCamera 208 | Value: true 209 | Views: 210 | Current: 211 | Angle: -3.21500063 212 | Class: rviz/TopDownOrtho 213 | Enable Stereo Rendering: 214 | Stereo Eye Separation: 0.0599999987 215 | Stereo Focal Distance: 1 216 | Swap Stereo Eyes: false 217 | Value: false 218 | Invert Z Axis: false 219 | Name: Current View 220 | Near Clip Distance: 0.00999999978 221 | Scale: 4.90428019 222 | Target Frame: 223 | Value: TopDownOrtho (rviz) 224 | X: 64.6689682 225 | Y: -26.2023258 226 | Saved: ~ 227 | Window Geometry: 228 | Displays: 229 | collapsed: false 230 | Height: 907 231 | Help: 232 | collapsed: false 233 | Hide Left Dock: false 234 | Hide Right Dock: true 235 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000345fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000345000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000800480065006c0070000000032c000000ba0000007300ffffff000000010000010f000003befc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003be000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f00000039fc0100000003fb0000001600520061006e0067006500200049006d00610067006500000000000000073f0000000000000000fb0000000a0049006d0061006700650000000000000006100000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073e0000003efc0100000002fb0000000800540069006d006500000000000000073e0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000034500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 236 | Selection: 237 | collapsed: false 238 | Time: 239 | collapsed: false 240 | Tool Properties: 241 | collapsed: false 242 | Views: 243 | collapsed: true 244 | Width: 1855 245 | X: 575 246 | Y: 64 247 | -------------------------------------------------------------------------------- /LeGO-LOAM/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lego_loam_bor 4 | 1.0.0 5 | Fork of LeGO-LOAM 6 | 7 | Davide Faconti 8 | 9 | BSD 10 | 11 | Tixiao Shan 12 | Davide Faconti 13 | 14 | catkin 15 | 16 | roscpp 17 | roscpp 18 | 19 | rospy 20 | rospy 21 | 22 | ros_bridge 23 | ros_bridge 24 | 25 | rosbag_storage 26 | rosbag_storage 27 | 28 | tf 29 | tf 30 | 31 | pcl_ros 32 | pcl_ros 33 | pcl_conversions 34 | pcl_conversions 35 | 36 | cv_bridge 37 | cv_bridge 38 | 39 | std_msgs 40 | std_msgs 41 | cloud_msgs 42 | cloud_msgs 43 | sensors_msgs 44 | sensors_msgs 45 | geometry_msgs 46 | geometry_msgs 47 | nav_msgs 48 | nav_msgs 49 | 50 | image_transport 51 | image_transport 52 | 53 | gtsam 54 | gtsam 55 | 56 | 57 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/featureAssociation.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 papers: 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 | // T. Shan and B. Englot. LeGO-LOAM: Lightweight and Ground-Optimized Lidar 33 | // Odometry and Mapping on Variable Terrain 34 | // IEEE/RSJ International Conference on Intelligent Robots and Systems 35 | // (IROS). October 2018. 36 | 37 | #include "featureAssociation.h" 38 | 39 | const float RAD2DEG = 180.0 / M_PI; 40 | 41 | FeatureAssociation::FeatureAssociation(ros::NodeHandle &node, 42 | Channel &input_channel, 43 | Channel &output_channel) 44 | : nh(node), 45 | _input_channel(input_channel), 46 | _output_channel(output_channel) { 47 | 48 | pubCornerPointsSharp = 49 | nh.advertise("/laser_cloud_sharp", 1); 50 | pubCornerPointsLessSharp = 51 | nh.advertise("/laser_cloud_less_sharp", 1); 52 | pubSurfPointsFlat = 53 | nh.advertise("/laser_cloud_flat", 1); 54 | pubSurfPointsLessFlat = 55 | nh.advertise("/laser_cloud_less_flat", 1); 56 | 57 | _pub_cloud_corner_last = 58 | nh.advertise("/laser_cloud_corner_last", 2); 59 | _pub_cloud_surf_last = 60 | nh.advertise("/laser_cloud_surf_last", 2); 61 | _pub_outlier_cloudLast = 62 | nh.advertise("/outlier_cloud_last", 2); 63 | pubLaserOdometry = nh.advertise("/laser_odom_to_init", 5); 64 | 65 | _cycle_count = 0; 66 | 67 | nh.getParam("/lego_loam/laser/num_vertical_scans", _vertical_scans); 68 | nh.getParam("/lego_loam/laser/num_horizontal_scans", _horizontal_scans); 69 | nh.getParam("/lego_loam/laser/scan_period", _scan_period); 70 | 71 | nh.getParam("/lego_loam/featureAssociation/edge_threshold", _edge_threshold); 72 | nh.getParam("/lego_loam/featureAssociation/surf_threshold", _surf_threshold); 73 | 74 | nh.getParam("/lego_loam/mapping/mapping_frequency_divider", _mapping_frequency_div); 75 | 76 | float nearest_dist; 77 | nh.getParam("/lego_loam/featureAssociation/nearest_feature_search_distance", nearest_dist); 78 | _nearest_feature_dist_sqr = nearest_dist*nearest_dist; 79 | 80 | initializationValue(); 81 | 82 | _run_thread = std::thread (&FeatureAssociation::runFeatureAssociation, this); 83 | } 84 | 85 | FeatureAssociation::~FeatureAssociation() 86 | { 87 | _input_channel.send({}); 88 | _run_thread.join(); 89 | } 90 | 91 | void FeatureAssociation::initializationValue() { 92 | const size_t cloud_size = _vertical_scans * _horizontal_scans; 93 | cloudSmoothness.resize(cloud_size); 94 | 95 | downSizeFilter.setLeafSize(0.2, 0.2, 0.2); 96 | 97 | segmentedCloud.reset(new pcl::PointCloud()); 98 | outlierCloud.reset(new pcl::PointCloud()); 99 | 100 | cornerPointsSharp.reset(new pcl::PointCloud()); 101 | cornerPointsLessSharp.reset(new pcl::PointCloud()); 102 | surfPointsFlat.reset(new pcl::PointCloud()); 103 | surfPointsLessFlat.reset(new pcl::PointCloud()); 104 | 105 | surfPointsLessFlatScan.reset(new pcl::PointCloud()); 106 | surfPointsLessFlatScanDS.reset(new pcl::PointCloud()); 107 | 108 | cloudCurvature.resize(cloud_size); 109 | cloudNeighborPicked.resize(cloud_size); 110 | cloudLabel.resize(cloud_size); 111 | 112 | pointSelCornerInd.resize(cloud_size); 113 | pointSearchCornerInd1.resize(cloud_size); 114 | pointSearchCornerInd2.resize(cloud_size); 115 | 116 | pointSelSurfInd.resize(cloud_size); 117 | pointSearchSurfInd1.resize(cloud_size); 118 | pointSearchSurfInd2.resize(cloud_size); 119 | pointSearchSurfInd3.resize(cloud_size); 120 | 121 | systemInitCount = 0; 122 | systemInited = false; 123 | 124 | skipFrameNum = 1; 125 | 126 | for (int i = 0; i < 6; ++i) { 127 | transformCur[i] = 0; 128 | transformSum[i] = 0; 129 | } 130 | 131 | systemInitedLM = false; 132 | 133 | laserCloudCornerLast.reset(new pcl::PointCloud()); 134 | laserCloudSurfLast.reset(new pcl::PointCloud()); 135 | laserCloudOri.reset(new pcl::PointCloud()); 136 | coeffSel.reset(new pcl::PointCloud()); 137 | 138 | laserOdometry.header.frame_id = "/camera_init"; 139 | laserOdometry.child_frame_id = "/laser_odom"; 140 | 141 | laserOdometryTrans.frame_id_ = "/camera_init"; 142 | laserOdometryTrans.child_frame_id_ = "/laser_odom"; 143 | 144 | isDegenerate = false; 145 | 146 | frameCount = skipFrameNum; 147 | } 148 | 149 | void FeatureAssociation::adjustDistortion() { 150 | bool halfPassed = false; 151 | int cloudSize = segmentedCloud->points.size(); 152 | 153 | PointType point; 154 | 155 | for (int i = 0; i < cloudSize; i++) { 156 | point.x = segmentedCloud->points[i].y; 157 | point.y = segmentedCloud->points[i].z; 158 | point.z = segmentedCloud->points[i].x; 159 | 160 | float ori = -atan2(point.x, point.z); 161 | if (!halfPassed) { 162 | if (ori < segInfo.startOrientation - M_PI / 2) 163 | ori += 2 * M_PI; 164 | else if (ori > segInfo.startOrientation + M_PI * 3 / 2) 165 | ori -= 2 * M_PI; 166 | 167 | if (ori - segInfo.startOrientation > M_PI) halfPassed = true; 168 | } else { 169 | ori += 2 * M_PI; 170 | 171 | if (ori < segInfo.endOrientation - M_PI * 3 / 2) 172 | ori += 2 * M_PI; 173 | else if (ori > segInfo.endOrientation + M_PI / 2) 174 | ori -= 2 * M_PI; 175 | } 176 | 177 | float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff; 178 | point.intensity = 179 | int(segmentedCloud->points[i].intensity) + _scan_period * relTime; 180 | 181 | segmentedCloud->points[i] = point; 182 | } 183 | } 184 | 185 | void FeatureAssociation::calculateSmoothness() { 186 | int cloudSize = segmentedCloud->points.size(); 187 | for (int i = 5; i < cloudSize - 5; i++) { 188 | float diffRange = segInfo.segmentedCloudRange[i - 5] + 189 | segInfo.segmentedCloudRange[i - 4] + 190 | segInfo.segmentedCloudRange[i - 3] + 191 | segInfo.segmentedCloudRange[i - 2] + 192 | segInfo.segmentedCloudRange[i - 1] - 193 | segInfo.segmentedCloudRange[i] * 10 + 194 | segInfo.segmentedCloudRange[i + 1] + 195 | segInfo.segmentedCloudRange[i + 2] + 196 | segInfo.segmentedCloudRange[i + 3] + 197 | segInfo.segmentedCloudRange[i + 4] + 198 | segInfo.segmentedCloudRange[i + 5]; 199 | 200 | cloudCurvature[i] = diffRange * diffRange; 201 | 202 | cloudNeighborPicked[i] = 0; 203 | cloudLabel[i] = 0; 204 | 205 | cloudSmoothness[i].value = cloudCurvature[i]; 206 | cloudSmoothness[i].ind = i; 207 | } 208 | } 209 | 210 | void FeatureAssociation::markOccludedPoints() { 211 | int cloudSize = segmentedCloud->points.size(); 212 | 213 | for (int i = 5; i < cloudSize - 6; ++i) { 214 | float depth1 = segInfo.segmentedCloudRange[i]; 215 | float depth2 = segInfo.segmentedCloudRange[i + 1]; 216 | int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i + 1] - 217 | segInfo.segmentedCloudColInd[i])); 218 | 219 | if (columnDiff < 10) { 220 | if (depth1 - depth2 > 0.3) { 221 | cloudNeighborPicked[i - 5] = 1; 222 | cloudNeighborPicked[i - 4] = 1; 223 | cloudNeighborPicked[i - 3] = 1; 224 | cloudNeighborPicked[i - 2] = 1; 225 | cloudNeighborPicked[i - 1] = 1; 226 | cloudNeighborPicked[i] = 1; 227 | } else if (depth2 - depth1 > 0.3) { 228 | cloudNeighborPicked[i + 1] = 1; 229 | cloudNeighborPicked[i + 2] = 1; 230 | cloudNeighborPicked[i + 3] = 1; 231 | cloudNeighborPicked[i + 4] = 1; 232 | cloudNeighborPicked[i + 5] = 1; 233 | cloudNeighborPicked[i + 6] = 1; 234 | } 235 | } 236 | 237 | float diff1 = std::abs(segInfo.segmentedCloudRange[i - 1] - 238 | segInfo.segmentedCloudRange[i]); 239 | float diff2 = std::abs(segInfo.segmentedCloudRange[i + 1] - 240 | segInfo.segmentedCloudRange[i]); 241 | 242 | if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && 243 | diff2 > 0.02 * segInfo.segmentedCloudRange[i]) 244 | cloudNeighborPicked[i] = 1; 245 | } 246 | } 247 | 248 | void FeatureAssociation::extractFeatures() { 249 | cornerPointsSharp->clear(); 250 | cornerPointsLessSharp->clear(); 251 | surfPointsFlat->clear(); 252 | surfPointsLessFlat->clear(); 253 | 254 | for (int i = 0; i < _vertical_scans; i++) { 255 | surfPointsLessFlatScan->clear(); 256 | 257 | for (int j = 0; j < 6; j++) { 258 | int sp = 259 | (segInfo.startRingIndex[i] * (6 - j) + segInfo.endRingIndex[i] * j) / 260 | 6; 261 | int ep = (segInfo.startRingIndex[i] * (5 - j) + 262 | segInfo.endRingIndex[i] * (j + 1)) / 263 | 6 - 264 | 1; 265 | 266 | if (sp >= ep) continue; 267 | 268 | std::sort(cloudSmoothness.begin() + sp, cloudSmoothness.begin() + ep, 269 | by_value()); 270 | 271 | int largestPickedNum = 0; 272 | for (int k = ep; k >= sp; k--) { 273 | int ind = cloudSmoothness[k].ind; 274 | if (cloudNeighborPicked[ind] == 0 && 275 | cloudCurvature[ind] > _edge_threshold && 276 | segInfo.segmentedCloudGroundFlag[ind] == false) { 277 | largestPickedNum++; 278 | if (largestPickedNum <= 2) { 279 | cloudLabel[ind] = 2; 280 | cornerPointsSharp->push_back(segmentedCloud->points[ind]); 281 | cornerPointsLessSharp->push_back(segmentedCloud->points[ind]); 282 | } else if (largestPickedNum <= 20) { 283 | cloudLabel[ind] = 1; 284 | cornerPointsLessSharp->push_back(segmentedCloud->points[ind]); 285 | } else { 286 | break; 287 | } 288 | 289 | cloudNeighborPicked[ind] = 1; 290 | for (int l = 1; l <= 5; l++) { 291 | if( ind + l >= segInfo.segmentedCloudColInd.size() ) { 292 | continue; 293 | } 294 | int columnDiff = 295 | std::abs(int(segInfo.segmentedCloudColInd[ind + l] - 296 | segInfo.segmentedCloudColInd[ind + l - 1])); 297 | if (columnDiff > 10) break; 298 | cloudNeighborPicked[ind + l] = 1; 299 | } 300 | for (int l = -1; l >= -5; l--) { 301 | if( ind + l < 0 ) { 302 | continue; 303 | } 304 | int columnDiff = 305 | std::abs(int(segInfo.segmentedCloudColInd[ind + l] - 306 | segInfo.segmentedCloudColInd[ind + l + 1])); 307 | if (columnDiff > 10) break; 308 | cloudNeighborPicked[ind + l] = 1; 309 | } 310 | } 311 | } 312 | 313 | int smallestPickedNum = 0; 314 | for (int k = sp; k <= ep; k++) { 315 | int ind = cloudSmoothness[k].ind; 316 | if (cloudNeighborPicked[ind] == 0 && 317 | cloudCurvature[ind] < _surf_threshold && 318 | segInfo.segmentedCloudGroundFlag[ind] == true) { 319 | cloudLabel[ind] = -1; 320 | surfPointsFlat->push_back(segmentedCloud->points[ind]); 321 | 322 | smallestPickedNum++; 323 | if (smallestPickedNum >= 4) { 324 | break; 325 | } 326 | 327 | cloudNeighborPicked[ind] = 1; 328 | for (int l = 1; l <= 5; l++) { 329 | if( ind + l >= segInfo.segmentedCloudColInd.size() ) { 330 | continue; 331 | } 332 | int columnDiff = 333 | std::abs(int(segInfo.segmentedCloudColInd.at(ind + l) - 334 | segInfo.segmentedCloudColInd.at(ind + l - 1))); 335 | if (columnDiff > 10) break; 336 | 337 | cloudNeighborPicked[ind + l] = 1; 338 | } 339 | for (int l = -1; l >= -5; l--) { 340 | if (ind + l < 0) { 341 | continue; 342 | } 343 | int columnDiff = 344 | std::abs(int(segInfo.segmentedCloudColInd.at(ind + l) - 345 | segInfo.segmentedCloudColInd.at(ind + l + 1))); 346 | if (columnDiff > 10) break; 347 | 348 | cloudNeighborPicked[ind + l] = 1; 349 | } 350 | } 351 | } 352 | 353 | for (int k = sp; k <= ep; k++) { 354 | if (cloudLabel[k] <= 0) { 355 | surfPointsLessFlatScan->push_back(segmentedCloud->points[k]); 356 | } 357 | } 358 | } 359 | 360 | surfPointsLessFlatScanDS->clear(); 361 | downSizeFilter.setInputCloud(surfPointsLessFlatScan); 362 | downSizeFilter.filter(*surfPointsLessFlatScanDS); 363 | 364 | *surfPointsLessFlat += *surfPointsLessFlatScanDS; 365 | } 366 | } 367 | 368 | void FeatureAssociation::TransformToStart(PointType const *const pi, 369 | PointType *const po) { 370 | float s = 10 * (pi->intensity - int(pi->intensity)); 371 | 372 | float rx = s * transformCur[0]; 373 | float ry = s * transformCur[1]; 374 | float rz = s * transformCur[2]; 375 | float tx = s * transformCur[3]; 376 | float ty = s * transformCur[4]; 377 | float tz = s * transformCur[5]; 378 | 379 | float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty); 380 | float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty); 381 | float z1 = (pi->z - tz); 382 | 383 | float x2 = x1; 384 | float y2 = cos(rx) * y1 + sin(rx) * z1; 385 | float z2 = -sin(rx) * y1 + cos(rx) * z1; 386 | 387 | po->x = cos(ry) * x2 - sin(ry) * z2; 388 | po->y = y2; 389 | po->z = sin(ry) * x2 + cos(ry) * z2; 390 | po->intensity = pi->intensity; 391 | } 392 | 393 | void FeatureAssociation::TransformToEnd(PointType const *const pi, 394 | PointType *const po) { 395 | float s = 10 * (pi->intensity - int(pi->intensity)); 396 | 397 | float rx = s * transformCur[0]; 398 | float ry = s * transformCur[1]; 399 | float rz = s * transformCur[2]; 400 | float tx = s * transformCur[3]; 401 | float ty = s * transformCur[4]; 402 | float tz = s * transformCur[5]; 403 | 404 | float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty); 405 | float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty); 406 | float z1 = (pi->z - tz); 407 | 408 | float x2 = x1; 409 | float y2 = cos(rx) * y1 + sin(rx) * z1; 410 | float z2 = -sin(rx) * y1 + cos(rx) * z1; 411 | 412 | float x3 = cos(ry) * x2 - sin(ry) * z2; 413 | float y3 = y2; 414 | float z3 = sin(ry) * x2 + cos(ry) * z2; 415 | 416 | rx = transformCur[0]; 417 | ry = transformCur[1]; 418 | rz = transformCur[2]; 419 | tx = transformCur[3]; 420 | ty = transformCur[4]; 421 | tz = transformCur[5]; 422 | 423 | float x4 = cos(ry) * x3 + sin(ry) * z3; 424 | float y4 = y3; 425 | float z4 = -sin(ry) * x3 + cos(ry) * z3; 426 | 427 | float x5 = x4; 428 | float y5 = cos(rx) * y4 - sin(rx) * z4; 429 | float z5 = sin(rx) * y4 + cos(rx) * z4; 430 | 431 | float x6 = cos(rz) * x5 - sin(rz) * y5 + tx; 432 | float y6 = sin(rz) * x5 + cos(rz) * y5 + ty; 433 | float z6 = z5 + tz; 434 | 435 | po->x = x6; 436 | po->y = y6; 437 | po->z = z6; 438 | po->intensity = int(pi->intensity); 439 | } 440 | 441 | 442 | void FeatureAssociation::AccumulateRotation(float cx, float cy, float cz, 443 | float lx, float ly, float lz, 444 | float &ox, float &oy, float &oz) { 445 | float srx = cos(lx) * cos(cx) * sin(ly) * sin(cz) - 446 | cos(cx) * cos(cz) * sin(lx) - cos(lx) * cos(ly) * sin(cx); 447 | ox = -asin(srx); 448 | 449 | float srycrx = 450 | sin(lx) * (cos(cy) * sin(cz) - cos(cz) * sin(cx) * sin(cy)) + 451 | cos(lx) * sin(ly) * (cos(cy) * cos(cz) + sin(cx) * sin(cy) * sin(cz)) + 452 | cos(lx) * cos(ly) * cos(cx) * sin(cy); 453 | float crycrx = 454 | cos(lx) * cos(ly) * cos(cx) * cos(cy) - 455 | cos(lx) * sin(ly) * (cos(cz) * sin(cy) - cos(cy) * sin(cx) * sin(cz)) - 456 | sin(lx) * (sin(cy) * sin(cz) + cos(cy) * cos(cz) * sin(cx)); 457 | oy = atan2(srycrx / cos(ox), crycrx / cos(ox)); 458 | 459 | float srzcrx = 460 | sin(cx) * (cos(lz) * sin(ly) - cos(ly) * sin(lx) * sin(lz)) + 461 | cos(cx) * sin(cz) * (cos(ly) * cos(lz) + sin(lx) * sin(ly) * sin(lz)) + 462 | cos(lx) * cos(cx) * cos(cz) * sin(lz); 463 | float crzcrx = 464 | cos(lx) * cos(lz) * cos(cx) * cos(cz) - 465 | cos(cx) * sin(cz) * (cos(ly) * sin(lz) - cos(lz) * sin(lx) * sin(ly)) - 466 | sin(cx) * (sin(ly) * sin(lz) + cos(ly) * cos(lz) * sin(lx)); 467 | oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox)); 468 | } 469 | 470 | void FeatureAssociation::findCorrespondingCornerFeatures(int iterCount) { 471 | int cornerPointsSharpNum = cornerPointsSharp->points.size(); 472 | 473 | for (int i = 0; i < cornerPointsSharpNum; i++) { 474 | PointType pointSel; 475 | TransformToStart(&cornerPointsSharp->points[i], &pointSel); 476 | 477 | if (iterCount % 5 == 0) { 478 | kdtreeCornerLast.nearestKSearch(pointSel, 1, pointSearchInd, 479 | pointSearchSqDis); 480 | int closestPointInd = -1, minPointInd2 = -1; 481 | 482 | if (pointSearchSqDis[0] < _nearest_feature_dist_sqr) { 483 | closestPointInd = pointSearchInd[0]; 484 | int closestPointScan = 485 | int(laserCloudCornerLast->points[closestPointInd].intensity); 486 | 487 | float pointSqDis, minPointSqDis2 = _nearest_feature_dist_sqr; 488 | for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) { 489 | if (int(laserCloudCornerLast->points[j].intensity) > 490 | closestPointScan + 2.5) { 491 | break; 492 | } 493 | 494 | pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 495 | (laserCloudCornerLast->points[j].x - pointSel.x) + 496 | (laserCloudCornerLast->points[j].y - pointSel.y) * 497 | (laserCloudCornerLast->points[j].y - pointSel.y) + 498 | (laserCloudCornerLast->points[j].z - pointSel.z) * 499 | (laserCloudCornerLast->points[j].z - pointSel.z); 500 | 501 | if (int(laserCloudCornerLast->points[j].intensity) > 502 | closestPointScan) { 503 | if (pointSqDis < minPointSqDis2) { 504 | minPointSqDis2 = pointSqDis; 505 | minPointInd2 = j; 506 | } 507 | } 508 | } 509 | for (int j = closestPointInd - 1; j >= 0; j--) { 510 | if (int(laserCloudCornerLast->points[j].intensity) < 511 | closestPointScan - 2.5) { 512 | break; 513 | } 514 | 515 | pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 516 | (laserCloudCornerLast->points[j].x - pointSel.x) + 517 | (laserCloudCornerLast->points[j].y - pointSel.y) * 518 | (laserCloudCornerLast->points[j].y - pointSel.y) + 519 | (laserCloudCornerLast->points[j].z - pointSel.z) * 520 | (laserCloudCornerLast->points[j].z - pointSel.z); 521 | 522 | if (int(laserCloudCornerLast->points[j].intensity) < 523 | closestPointScan) { 524 | if (pointSqDis < minPointSqDis2) { 525 | minPointSqDis2 = pointSqDis; 526 | minPointInd2 = j; 527 | } 528 | } 529 | } 530 | } 531 | 532 | pointSearchCornerInd1[i] = closestPointInd; 533 | pointSearchCornerInd2[i] = minPointInd2; 534 | } 535 | 536 | if (pointSearchCornerInd2[i] >= 0) { 537 | PointType tripod1 = 538 | laserCloudCornerLast->points[pointSearchCornerInd1[i]]; 539 | PointType tripod2 = 540 | laserCloudCornerLast->points[pointSearchCornerInd2[i]]; 541 | 542 | float x0 = pointSel.x; 543 | float y0 = pointSel.y; 544 | float z0 = pointSel.z; 545 | float x1 = tripod1.x; 546 | float y1 = tripod1.y; 547 | float z1 = tripod1.z; 548 | float x2 = tripod2.x; 549 | float y2 = tripod2.y; 550 | float z2 = tripod2.z; 551 | 552 | float m11 = ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)); 553 | float m22 = ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)); 554 | float m33 = ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)); 555 | 556 | float a012 = sqrt(m11 * m11 + m22 * m22 + m33 * m33); 557 | 558 | float l12 = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + 559 | (z1 - z2) * (z1 - z2)); 560 | 561 | float la = ((y1 - y2) * m11 + (z1 - z2) * m22) / a012 / l12; 562 | 563 | float lb = -((x1 - x2) * m11 - (z1 - z2) * m33) / a012 / l12; 564 | 565 | float lc = -((x1 - x2) * m22 + (y1 - y2) * m33) / a012 / l12; 566 | 567 | float ld2 = a012 / l12; 568 | 569 | float s = 1; 570 | if (iterCount >= 5) { 571 | s = 1 - 1.8 * fabs(ld2); 572 | } 573 | 574 | if (s > 0.1 && ld2 != 0) { 575 | PointType coeff; 576 | coeff.x = s * la; 577 | coeff.y = s * lb; 578 | coeff.z = s * lc; 579 | coeff.intensity = s * ld2; 580 | 581 | laserCloudOri->push_back(cornerPointsSharp->points[i]); 582 | coeffSel->push_back(coeff); 583 | } 584 | } 585 | } 586 | } 587 | 588 | void FeatureAssociation::findCorrespondingSurfFeatures(int iterCount) { 589 | int surfPointsFlatNum = surfPointsFlat->points.size(); 590 | 591 | for (int i = 0; i < surfPointsFlatNum; i++) { 592 | PointType pointSel; 593 | TransformToStart(&surfPointsFlat->points[i], &pointSel); 594 | 595 | if (iterCount % 5 == 0) { 596 | kdtreeSurfLast.nearestKSearch(pointSel, 1, pointSearchInd, 597 | pointSearchSqDis); 598 | int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; 599 | 600 | if (pointSearchSqDis[0] < _nearest_feature_dist_sqr) { 601 | closestPointInd = pointSearchInd[0]; 602 | int closestPointScan = 603 | int(laserCloudSurfLast->points[closestPointInd].intensity); 604 | 605 | float pointSqDis, minPointSqDis2 = _nearest_feature_dist_sqr, 606 | minPointSqDis3 = _nearest_feature_dist_sqr; 607 | for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) { 608 | if (int(laserCloudSurfLast->points[j].intensity) > 609 | closestPointScan + 2.5) { 610 | break; 611 | } 612 | 613 | pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 614 | (laserCloudSurfLast->points[j].x - pointSel.x) + 615 | (laserCloudSurfLast->points[j].y - pointSel.y) * 616 | (laserCloudSurfLast->points[j].y - pointSel.y) + 617 | (laserCloudSurfLast->points[j].z - pointSel.z) * 618 | (laserCloudSurfLast->points[j].z - pointSel.z); 619 | 620 | if (int(laserCloudSurfLast->points[j].intensity) <= 621 | closestPointScan) { 622 | if (pointSqDis < minPointSqDis2) { 623 | minPointSqDis2 = pointSqDis; 624 | minPointInd2 = j; 625 | } 626 | } else { 627 | if (pointSqDis < minPointSqDis3) { 628 | minPointSqDis3 = pointSqDis; 629 | minPointInd3 = j; 630 | } 631 | } 632 | } 633 | for (int j = closestPointInd - 1; j >= 0; j--) { 634 | if (int(laserCloudSurfLast->points[j].intensity) < 635 | closestPointScan - 2.5) { 636 | break; 637 | } 638 | 639 | pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 640 | (laserCloudSurfLast->points[j].x - pointSel.x) + 641 | (laserCloudSurfLast->points[j].y - pointSel.y) * 642 | (laserCloudSurfLast->points[j].y - pointSel.y) + 643 | (laserCloudSurfLast->points[j].z - pointSel.z) * 644 | (laserCloudSurfLast->points[j].z - pointSel.z); 645 | 646 | if (int(laserCloudSurfLast->points[j].intensity) >= 647 | closestPointScan) { 648 | if (pointSqDis < minPointSqDis2) { 649 | minPointSqDis2 = pointSqDis; 650 | minPointInd2 = j; 651 | } 652 | } else { 653 | if (pointSqDis < minPointSqDis3) { 654 | minPointSqDis3 = pointSqDis; 655 | minPointInd3 = j; 656 | } 657 | } 658 | } 659 | } 660 | 661 | pointSearchSurfInd1[i] = closestPointInd; 662 | pointSearchSurfInd2[i] = minPointInd2; 663 | pointSearchSurfInd3[i] = minPointInd3; 664 | } 665 | 666 | if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) { 667 | PointType tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]]; 668 | PointType tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]]; 669 | PointType tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]]; 670 | 671 | float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) - 672 | (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z); 673 | float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) - 674 | (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x); 675 | float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) - 676 | (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y); 677 | float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z); 678 | 679 | float ps = sqrt(pa * pa + pb * pb + pc * pc); 680 | 681 | pa /= ps; 682 | pb /= ps; 683 | pc /= ps; 684 | pd /= ps; 685 | 686 | float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; 687 | 688 | float s = 1; 689 | if (iterCount >= 5) { 690 | s = 1 - 691 | 1.8 * fabs(pd2) / 692 | sqrt(sqrt(pointSel.x * pointSel.x + pointSel.y * pointSel.y + 693 | pointSel.z * pointSel.z)); 694 | } 695 | 696 | if (s > 0.1 && pd2 != 0) { 697 | PointType coeff; 698 | coeff.x = s * pa; 699 | coeff.y = s * pb; 700 | coeff.z = s * pc; 701 | coeff.intensity = s * pd2; 702 | 703 | laserCloudOri->push_back(surfPointsFlat->points[i]); 704 | coeffSel->push_back(coeff); 705 | } 706 | } 707 | } 708 | } 709 | 710 | bool FeatureAssociation::calculateTransformationSurf(int iterCount) { 711 | int pointSelNum = laserCloudOri->points.size(); 712 | 713 | Eigen::Matrix matA(pointSelNum, 3); 714 | Eigen::Matrix matAt(3,pointSelNum); 715 | Eigen::Matrix matAtA; 716 | Eigen::VectorXf matB(pointSelNum); 717 | Eigen::Matrix matAtB; 718 | Eigen::Matrix matX; 719 | Eigen::Matrix matP; 720 | 721 | float srx = sin(transformCur[0]); 722 | float crx = cos(transformCur[0]); 723 | float sry = sin(transformCur[1]); 724 | float cry = cos(transformCur[1]); 725 | float srz = sin(transformCur[2]); 726 | float crz = cos(transformCur[2]); 727 | float tx = transformCur[3]; 728 | float ty = transformCur[4]; 729 | float tz = transformCur[5]; 730 | 731 | float a1 = crx * sry * srz; 732 | float a2 = crx * crz * sry; 733 | float a3 = srx * sry; 734 | float a4 = tx * a1 - ty * a2 - tz * a3; 735 | float a5 = srx * srz; 736 | float a6 = crz * srx; 737 | float a7 = ty * a6 - tz * crx - tx * a5; 738 | float a8 = crx * cry * srz; 739 | float a9 = crx * cry * crz; 740 | float a10 = cry * srx; 741 | float a11 = tz * a10 + ty * a9 - tx * a8; 742 | 743 | float b1 = -crz * sry - cry * srx * srz; 744 | float b2 = cry * crz * srx - sry * srz; 745 | float b5 = cry * crz - srx * sry * srz; 746 | float b6 = cry * srz + crz * srx * sry; 747 | 748 | float c1 = -b6; 749 | float c2 = b5; 750 | float c3 = tx * b6 - ty * b5; 751 | float c4 = -crx * crz; 752 | float c5 = crx * srz; 753 | float c6 = ty * c5 + tx * -c4; 754 | float c7 = b2; 755 | float c8 = -b1; 756 | float c9 = tx * -b2 - ty * -b1; 757 | 758 | for (int i = 0; i < pointSelNum; i++) { 759 | PointType pointOri = laserCloudOri->points[i]; 760 | PointType coeff = coeffSel->points[i]; 761 | 762 | float arx = 763 | (-a1 * pointOri.x + a2 * pointOri.y + a3 * pointOri.z + a4) * coeff.x + 764 | (a5 * pointOri.x - a6 * pointOri.y + crx * pointOri.z + a7) * coeff.y + 765 | (a8 * pointOri.x - a9 * pointOri.y - a10 * pointOri.z + a11) * coeff.z; 766 | 767 | float arz = (c1 * pointOri.x + c2 * pointOri.y + c3) * coeff.x + 768 | (c4 * pointOri.x - c5 * pointOri.y + c6) * coeff.y + 769 | (c7 * pointOri.x + c8 * pointOri.y + c9) * coeff.z; 770 | 771 | float aty = -b6 * coeff.x + c4 * coeff.y + b2 * coeff.z; 772 | 773 | float d2 = coeff.intensity; 774 | 775 | matA(i, 0) = arx; 776 | matA(i, 1) = arz; 777 | matA(i, 2) = aty; 778 | matB(i, 0) = -0.05 * d2; 779 | } 780 | 781 | matAt = matA.transpose(); 782 | matAtA = matAt * matA; 783 | matAtB = matAt * matB; 784 | matX = matAtA.colPivHouseholderQr().solve(matAtB); 785 | 786 | if (iterCount == 0) { 787 | Eigen::Matrix matE; 788 | Eigen::Matrix matV; 789 | Eigen::Matrix matV2; 790 | 791 | Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA); 792 | matE = esolver.eigenvalues().real(); 793 | matV = esolver.eigenvectors().real(); 794 | matV2 = matV; 795 | 796 | isDegenerate = false; 797 | float eignThre[3] = {10, 10, 10}; 798 | for (int i = 2; i >= 0; i--) { 799 | if (matE(0, i) < eignThre[i]) { 800 | for (int j = 0; j < 3; j++) { 801 | matV2(i, j) = 0; 802 | } 803 | isDegenerate = true; 804 | } else { 805 | break; 806 | } 807 | } 808 | matP = matV.inverse() * matV2; 809 | } 810 | 811 | if (isDegenerate) { 812 | Eigen::Matrix matX2; 813 | matX2 = matX; 814 | matX = matP * matX2; 815 | } 816 | 817 | transformCur[0] += matX(0, 0); 818 | transformCur[2] += matX(1, 0); 819 | transformCur[4] += matX(2, 0); 820 | 821 | for (int i = 0; i < 6; i++) { 822 | if (std::isnan(transformCur[i])) transformCur[i] = 0; 823 | } 824 | 825 | float deltaR = sqrt(pow(RAD2DEG * (matX(0, 0)), 2) + 826 | pow(RAD2DEG * (matX(1, 0)), 2)); 827 | float deltaT = sqrt(pow(matX(2, 0) * 100, 2)); 828 | 829 | if (deltaR < 0.1 && deltaT < 0.1) { 830 | return false; 831 | } 832 | return true; 833 | } 834 | 835 | bool FeatureAssociation::calculateTransformationCorner(int iterCount) { 836 | int pointSelNum = laserCloudOri->points.size(); 837 | 838 | Eigen::Matrix matA(pointSelNum, 3); 839 | Eigen::Matrix matAt(3,pointSelNum); 840 | Eigen::Matrix matAtA; 841 | Eigen::VectorXf matB(pointSelNum); 842 | Eigen::Matrix matAtB; 843 | Eigen::Matrix matX; 844 | Eigen::Matrix matP; 845 | 846 | float srx = sin(transformCur[0]); 847 | float crx = cos(transformCur[0]); 848 | float sry = sin(transformCur[1]); 849 | float cry = cos(transformCur[1]); 850 | float srz = sin(transformCur[2]); 851 | float crz = cos(transformCur[2]); 852 | float tx = transformCur[3]; 853 | float ty = transformCur[4]; 854 | float tz = transformCur[5]; 855 | 856 | float b1 = -crz * sry - cry * srx * srz; 857 | float b2 = cry * crz * srx - sry * srz; 858 | float b3 = crx * cry; 859 | float b4 = tx * -b1 + ty * -b2 + tz * b3; 860 | float b5 = cry * crz - srx * sry * srz; 861 | float b6 = cry * srz + crz * srx * sry; 862 | float b7 = crx * sry; 863 | float b8 = tz * b7 - ty * b6 - tx * b5; 864 | 865 | float c5 = crx * srz; 866 | 867 | for (int i = 0; i < pointSelNum; i++) { 868 | PointType pointOri = laserCloudOri->points[i]; 869 | PointType coeff = coeffSel->points[i]; 870 | 871 | float ary = 872 | (b1 * pointOri.x + b2 * pointOri.y - b3 * pointOri.z + b4) * coeff.x + 873 | (b5 * pointOri.x + b6 * pointOri.y - b7 * pointOri.z + b8) * coeff.z; 874 | 875 | float atx = -b5 * coeff.x + c5 * coeff.y + b1 * coeff.z; 876 | 877 | float atz = b7 * coeff.x - srx * coeff.y - b3 * coeff.z; 878 | 879 | float d2 = coeff.intensity; 880 | 881 | matA(i, 0) = ary; 882 | matA(i, 1) = atx; 883 | matA(i, 2) = atz; 884 | matB(i, 0) = -0.05 * d2; 885 | } 886 | 887 | matAt = matA.transpose(); 888 | matAtA = matAt * matA; 889 | matAtB = matAt * matB; 890 | matX = matAtA.colPivHouseholderQr().solve(matAtB); 891 | 892 | if (iterCount == 0) { 893 | Eigen::Matrix matE; 894 | Eigen::Matrix matV; 895 | Eigen::Matrix matV2; 896 | 897 | Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA); 898 | matE = esolver.eigenvalues().real(); 899 | matV = esolver.eigenvectors().real(); 900 | matV2 = matV; 901 | 902 | isDegenerate = false; 903 | float eignThre[3] = {10, 10, 10}; 904 | for (int i = 2; i >= 0; i--) { 905 | if (matE(0, i) < eignThre[i]) { 906 | for (int j = 0; j < 3; j++) { 907 | matV2(i, j) = 0; 908 | } 909 | isDegenerate = true; 910 | } else { 911 | break; 912 | } 913 | } 914 | matP = matV.inverse() * matV2; 915 | } 916 | 917 | if (isDegenerate) { 918 | Eigen::Matrix matX2; 919 | matX2 = matX; 920 | matX = matP * matX2; 921 | } 922 | 923 | transformCur[1] += matX(0, 0); 924 | transformCur[3] += matX(1, 0); 925 | transformCur[5] += matX(2, 0); 926 | 927 | for (int i = 0; i < 6; i++) { 928 | if (std::isnan(transformCur[i])) transformCur[i] = 0; 929 | } 930 | 931 | float deltaR = sqrt(pow(RAD2DEG * (matX(0, 0)), 2)); 932 | float deltaT = sqrt(pow(matX(1, 0) * 100, 2) + 933 | pow(matX(2, 0) * 100, 2)); 934 | 935 | if (deltaR < 0.1 && deltaT < 0.1) { 936 | return false; 937 | } 938 | return true; 939 | } 940 | 941 | bool FeatureAssociation::calculateTransformation(int iterCount) { 942 | int pointSelNum = laserCloudOri->points.size(); 943 | 944 | Eigen::Matrix matA(pointSelNum, 6); 945 | Eigen::Matrix matAt(6,pointSelNum); 946 | Eigen::Matrix matAtA; 947 | Eigen::VectorXf matB(pointSelNum); 948 | Eigen::Matrix matAtB; 949 | Eigen::Matrix matX; 950 | Eigen::Matrix matP; 951 | 952 | float srx = sin(transformCur[0]); 953 | float crx = cos(transformCur[0]); 954 | float sry = sin(transformCur[1]); 955 | float cry = cos(transformCur[1]); 956 | float srz = sin(transformCur[2]); 957 | float crz = cos(transformCur[2]); 958 | float tx = transformCur[3]; 959 | float ty = transformCur[4]; 960 | float tz = transformCur[5]; 961 | 962 | float a1 = crx * sry * srz; 963 | float a2 = crx * crz * sry; 964 | float a3 = srx * sry; 965 | float a4 = tx * a1 - ty * a2 - tz * a3; 966 | float a5 = srx * srz; 967 | float a6 = crz * srx; 968 | float a7 = ty * a6 - tz * crx - tx * a5; 969 | float a8 = crx * cry * srz; 970 | float a9 = crx * cry * crz; 971 | float a10 = cry * srx; 972 | float a11 = tz * a10 + ty * a9 - tx * a8; 973 | 974 | float b1 = -crz * sry - cry * srx * srz; 975 | float b2 = cry * crz * srx - sry * srz; 976 | float b3 = crx * cry; 977 | float b4 = tx * -b1 + ty * -b2 + tz * b3; 978 | float b5 = cry * crz - srx * sry * srz; 979 | float b6 = cry * srz + crz * srx * sry; 980 | float b7 = crx * sry; 981 | float b8 = tz * b7 - ty * b6 - tx * b5; 982 | 983 | float c1 = -b6; 984 | float c2 = b5; 985 | float c3 = tx * b6 - ty * b5; 986 | float c4 = -crx * crz; 987 | float c5 = crx * srz; 988 | float c6 = ty * c5 + tx * -c4; 989 | float c7 = b2; 990 | float c8 = -b1; 991 | float c9 = tx * -b2 - ty * -b1; 992 | 993 | for (int i = 0; i < pointSelNum; i++) { 994 | PointType pointOri = laserCloudOri->points[i]; 995 | PointType coeff = coeffSel->points[i]; 996 | 997 | float arx = 998 | (-a1 * pointOri.x + a2 * pointOri.y + a3 * pointOri.z + a4) * coeff.x + 999 | (a5 * pointOri.x - a6 * pointOri.y + crx * pointOri.z + a7) * coeff.y + 1000 | (a8 * pointOri.x - a9 * pointOri.y - a10 * pointOri.z + a11) * coeff.z; 1001 | 1002 | float ary = 1003 | (b1 * pointOri.x + b2 * pointOri.y - b3 * pointOri.z + b4) * coeff.x + 1004 | (b5 * pointOri.x + b6 * pointOri.y - b7 * pointOri.z + b8) * coeff.z; 1005 | 1006 | float arz = (c1 * pointOri.x + c2 * pointOri.y + c3) * coeff.x + 1007 | (c4 * pointOri.x - c5 * pointOri.y + c6) * coeff.y + 1008 | (c7 * pointOri.x + c8 * pointOri.y + c9) * coeff.z; 1009 | 1010 | float atx = -b5 * coeff.x + c5 * coeff.y + b1 * coeff.z; 1011 | 1012 | float aty = -b6 * coeff.x + c4 * coeff.y + b2 * coeff.z; 1013 | 1014 | float atz = b7 * coeff.x - srx * coeff.y - b3 * coeff.z; 1015 | 1016 | float d2 = coeff.intensity; 1017 | 1018 | matA(i, 0) = arx; 1019 | matA(i, 1) = ary; 1020 | matA(i, 2) = arz; 1021 | matA(i, 3) = atx; 1022 | matA(i, 4) = aty; 1023 | matA(i, 5) = atz; 1024 | matB(i, 0) = -0.05 * d2; 1025 | } 1026 | 1027 | matAt = matA.transpose(); 1028 | matAtA = matAt * matA; 1029 | matAtB = matAt * matB; 1030 | matX = matAtA.colPivHouseholderQr().solve(matAtB); 1031 | 1032 | if (iterCount == 0) { 1033 | Eigen::Matrix matE; 1034 | Eigen::Matrix matV; 1035 | Eigen::Matrix matV2; 1036 | 1037 | Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA); 1038 | matE = esolver.eigenvalues().real(); 1039 | matV = esolver.eigenvectors().real(); 1040 | matV2 = matV; 1041 | 1042 | isDegenerate = false; 1043 | float eignThre[6] = {10, 10, 10, 10, 10, 10}; 1044 | for (int i = 5; i >= 0; i--) { 1045 | if (matE(0, i) < eignThre[i]) { 1046 | for (int j = 0; j < 6; j++) { 1047 | matV2(i, j) = 0; 1048 | } 1049 | isDegenerate = true; 1050 | } else { 1051 | break; 1052 | } 1053 | } 1054 | matP = matV.inverse() * matV2; 1055 | } 1056 | 1057 | if (isDegenerate) { 1058 | Eigen::Matrix matX2; 1059 | matX2 = matX; 1060 | matX = matP * matX2; 1061 | } 1062 | 1063 | transformCur[0] += matX(0, 0); 1064 | transformCur[1] += matX(1, 0); 1065 | transformCur[2] += matX(2, 0); 1066 | transformCur[3] += matX(3, 0); 1067 | transformCur[4] += matX(4, 0); 1068 | transformCur[5] += matX(5, 0); 1069 | 1070 | for (int i = 0; i < 6; i++) { 1071 | if (std::isnan(transformCur[i])) transformCur[i] = 0; 1072 | } 1073 | 1074 | float deltaR = sqrt(pow(RAD2DEG * (matX(0, 0)), 2) + 1075 | pow(RAD2DEG * (matX(1, 0)), 2) + 1076 | pow(RAD2DEG * (matX(2, 0)), 2)); 1077 | float deltaT = sqrt(pow(matX(3, 0) * 100, 2) + 1078 | pow(matX(4, 0) * 100, 2) + 1079 | pow(matX(5, 0) * 100, 2)); 1080 | 1081 | if (deltaR < 0.1 && deltaT < 0.1) { 1082 | return false; 1083 | } 1084 | return true; 1085 | } 1086 | 1087 | void FeatureAssociation::checkSystemInitialization() { 1088 | pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp; 1089 | cornerPointsLessSharp = laserCloudCornerLast; 1090 | laserCloudCornerLast = laserCloudTemp; 1091 | 1092 | laserCloudTemp = surfPointsLessFlat; 1093 | surfPointsLessFlat = laserCloudSurfLast; 1094 | laserCloudSurfLast = laserCloudTemp; 1095 | 1096 | kdtreeCornerLast.setInputCloud(laserCloudCornerLast); 1097 | kdtreeSurfLast.setInputCloud(laserCloudSurfLast); 1098 | 1099 | laserCloudCornerLastNum = laserCloudCornerLast->points.size(); 1100 | laserCloudSurfLastNum = laserCloudSurfLast->points.size(); 1101 | 1102 | sensor_msgs::PointCloud2 laserCloudCornerLast2; 1103 | pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); 1104 | laserCloudCornerLast2.header.stamp = cloudHeader.stamp; 1105 | laserCloudCornerLast2.header.frame_id = "/camera"; 1106 | _pub_cloud_corner_last.publish(laserCloudCornerLast2); 1107 | 1108 | sensor_msgs::PointCloud2 laserCloudSurfLast2; 1109 | pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); 1110 | laserCloudSurfLast2.header.stamp = cloudHeader.stamp; 1111 | laserCloudSurfLast2.header.frame_id = "/camera"; 1112 | _pub_cloud_surf_last.publish(laserCloudSurfLast2); 1113 | 1114 | systemInitedLM = true; 1115 | } 1116 | 1117 | void FeatureAssociation::updateTransformation() { 1118 | if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100) return; 1119 | 1120 | for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) { 1121 | laserCloudOri->clear(); 1122 | coeffSel->clear(); 1123 | 1124 | findCorrespondingSurfFeatures(iterCount1); 1125 | 1126 | if (laserCloudOri->points.size() < 10) continue; 1127 | if (calculateTransformationSurf(iterCount1) == false) break; 1128 | } 1129 | 1130 | for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) { 1131 | laserCloudOri->clear(); 1132 | coeffSel->clear(); 1133 | 1134 | findCorrespondingCornerFeatures(iterCount2); 1135 | 1136 | if (laserCloudOri->points.size() < 10) continue; 1137 | if (calculateTransformationCorner(iterCount2) == false) break; 1138 | } 1139 | } 1140 | 1141 | void FeatureAssociation::integrateTransformation() { 1142 | float rx, ry, rz, tx, ty, tz; 1143 | AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], 1144 | -transformCur[0], -transformCur[1], -transformCur[2], rx, 1145 | ry, rz); 1146 | 1147 | float x1 = cos(rz) * (transformCur[3] ) - 1148 | sin(rz) * (transformCur[4] ); 1149 | float y1 = sin(rz) * (transformCur[3] ) + 1150 | cos(rz) * (transformCur[4] ); 1151 | float z1 = transformCur[5]; 1152 | 1153 | float x2 = x1; 1154 | float y2 = cos(rx) * y1 - sin(rx) * z1; 1155 | float z2 = sin(rx) * y1 + cos(rx) * z1; 1156 | 1157 | tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2); 1158 | ty = transformSum[4] - y2; 1159 | tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2); 1160 | 1161 | transformSum[0] = rx; 1162 | transformSum[1] = ry; 1163 | transformSum[2] = rz; 1164 | transformSum[3] = tx; 1165 | transformSum[4] = ty; 1166 | transformSum[5] = tz; 1167 | } 1168 | 1169 | void FeatureAssociation::adjustOutlierCloud() { 1170 | PointType point; 1171 | int cloudSize = outlierCloud->points.size(); 1172 | for (int i = 0; i < cloudSize; ++i) { 1173 | point.x = outlierCloud->points[i].y; 1174 | point.y = outlierCloud->points[i].z; 1175 | point.z = outlierCloud->points[i].x; 1176 | point.intensity = outlierCloud->points[i].intensity; 1177 | outlierCloud->points[i] = point; 1178 | } 1179 | } 1180 | 1181 | void FeatureAssociation::publishOdometry() { 1182 | geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw( 1183 | transformSum[2], -transformSum[0], -transformSum[1]); 1184 | 1185 | laserOdometry.header.stamp = cloudHeader.stamp; 1186 | laserOdometry.pose.pose.orientation.x = -geoQuat.y; 1187 | laserOdometry.pose.pose.orientation.y = -geoQuat.z; 1188 | laserOdometry.pose.pose.orientation.z = geoQuat.x; 1189 | laserOdometry.pose.pose.orientation.w = geoQuat.w; 1190 | laserOdometry.pose.pose.position.x = transformSum[3]; 1191 | laserOdometry.pose.pose.position.y = transformSum[4]; 1192 | laserOdometry.pose.pose.position.z = transformSum[5]; 1193 | pubLaserOdometry.publish(laserOdometry); 1194 | 1195 | laserOdometryTrans.stamp_ = cloudHeader.stamp; 1196 | laserOdometryTrans.setRotation( 1197 | tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 1198 | laserOdometryTrans.setOrigin( 1199 | tf::Vector3(transformSum[3], transformSum[4], transformSum[5])); 1200 | tfBroadcaster.sendTransform(laserOdometryTrans); 1201 | } 1202 | 1203 | void FeatureAssociation::publishCloud() { 1204 | sensor_msgs::PointCloud2 laserCloudOutMsg; 1205 | 1206 | auto Publish = [&](ros::Publisher &pub, 1207 | const pcl::PointCloud::Ptr &cloud) { 1208 | if (pub.getNumSubscribers() != 0) { 1209 | pcl::toROSMsg(*cloud, laserCloudOutMsg); 1210 | laserCloudOutMsg.header.stamp = cloudHeader.stamp; 1211 | laserCloudOutMsg.header.frame_id = "/camera"; 1212 | pub.publish(laserCloudOutMsg); 1213 | } 1214 | }; 1215 | 1216 | Publish(pubCornerPointsSharp, cornerPointsSharp); 1217 | Publish(pubCornerPointsLessSharp, cornerPointsLessSharp); 1218 | Publish(pubSurfPointsFlat, surfPointsFlat); 1219 | Publish(pubSurfPointsLessFlat, surfPointsLessFlat); 1220 | } 1221 | 1222 | void FeatureAssociation::publishCloudsLast() { 1223 | 1224 | int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size(); 1225 | for (int i = 0; i < cornerPointsLessSharpNum; i++) { 1226 | TransformToEnd(&cornerPointsLessSharp->points[i], 1227 | &cornerPointsLessSharp->points[i]); 1228 | } 1229 | 1230 | int surfPointsLessFlatNum = surfPointsLessFlat->points.size(); 1231 | for (int i = 0; i < surfPointsLessFlatNum; i++) { 1232 | TransformToEnd(&surfPointsLessFlat->points[i], 1233 | &surfPointsLessFlat->points[i]); 1234 | } 1235 | 1236 | pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp; 1237 | cornerPointsLessSharp = laserCloudCornerLast; 1238 | laserCloudCornerLast = laserCloudTemp; 1239 | 1240 | laserCloudTemp = surfPointsLessFlat; 1241 | surfPointsLessFlat = laserCloudSurfLast; 1242 | laserCloudSurfLast = laserCloudTemp; 1243 | 1244 | laserCloudCornerLastNum = laserCloudCornerLast->points.size(); 1245 | laserCloudSurfLastNum = laserCloudSurfLast->points.size(); 1246 | 1247 | if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) { 1248 | kdtreeCornerLast.setInputCloud(laserCloudCornerLast); 1249 | kdtreeSurfLast.setInputCloud(laserCloudSurfLast); 1250 | } 1251 | 1252 | frameCount++; 1253 | adjustOutlierCloud(); 1254 | 1255 | if (frameCount >= skipFrameNum + 1) { 1256 | frameCount = 0; 1257 | sensor_msgs::PointCloud2 cloudTemp; 1258 | 1259 | auto Publish = [&](ros::Publisher &pub, 1260 | const pcl::PointCloud::Ptr &cloud) { 1261 | if (pub.getNumSubscribers() != 0) { 1262 | pcl::toROSMsg(*cloud, cloudTemp); 1263 | cloudTemp.header.stamp = cloudHeader.stamp; 1264 | cloudTemp.header.frame_id = "/camera"; 1265 | pub.publish(cloudTemp); 1266 | } 1267 | }; 1268 | 1269 | Publish(_pub_outlier_cloudLast, outlierCloud); 1270 | Publish(_pub_cloud_corner_last, laserCloudCornerLast); 1271 | Publish(_pub_cloud_surf_last, laserCloudSurfLast); 1272 | } 1273 | } 1274 | 1275 | void FeatureAssociation::runFeatureAssociation() { 1276 | while (ros::ok()) { 1277 | ProjectionOut projection; 1278 | _input_channel.receive(projection); 1279 | 1280 | if( !ros::ok() ) break; 1281 | 1282 | //-------------- 1283 | outlierCloud = projection.outlier_cloud; 1284 | segmentedCloud = projection.segmented_cloud; 1285 | segInfo = std::move(projection.seg_msg); 1286 | 1287 | cloudHeader = segInfo.header; 1288 | timeScanCur = cloudHeader.stamp.toSec(); 1289 | 1290 | /** 1. Feature Extraction */ 1291 | adjustDistortion(); 1292 | 1293 | calculateSmoothness(); 1294 | 1295 | markOccludedPoints(); 1296 | 1297 | extractFeatures(); 1298 | 1299 | publishCloud(); // cloud for visualization 1300 | 1301 | // Feature Association 1302 | if (!systemInitedLM) { 1303 | checkSystemInitialization(); 1304 | continue; 1305 | } 1306 | 1307 | updateTransformation(); 1308 | 1309 | integrateTransformation(); 1310 | 1311 | publishOdometry(); 1312 | 1313 | publishCloudsLast(); // cloud to mapOptimization 1314 | 1315 | //-------------- 1316 | _cycle_count++; 1317 | 1318 | if (_cycle_count == _mapping_frequency_div) { 1319 | _cycle_count = 0; 1320 | AssociationOut out; 1321 | out.cloud_corner_last.reset(new pcl::PointCloud()); 1322 | out.cloud_surf_last.reset(new pcl::PointCloud()); 1323 | out.cloud_outlier_last.reset(new pcl::PointCloud()); 1324 | 1325 | *out.cloud_corner_last = *laserCloudCornerLast; 1326 | *out.cloud_surf_last = *laserCloudSurfLast; 1327 | *out.cloud_outlier_last = *outlierCloud; 1328 | 1329 | out.laser_odometry = laserOdometry; 1330 | 1331 | _output_channel.send(std::move(out)); 1332 | } 1333 | } 1334 | } 1335 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/featureAssociation.h: -------------------------------------------------------------------------------- 1 | #ifndef FEATUREASSOCIATION_H 2 | #define FEATUREASSOCIATION_H 3 | 4 | #include "lego_loam/utility.h" 5 | #include "lego_loam/channel.h" 6 | #include "lego_loam/nanoflann_pcl.h" 7 | #include 8 | #include 9 | 10 | class FeatureAssociation { 11 | 12 | public: 13 | FeatureAssociation( ros::NodeHandle& node, 14 | Channel& input_channel, 15 | Channel& output_channel); 16 | 17 | ~FeatureAssociation(); 18 | 19 | void runFeatureAssociation(); 20 | 21 | private: 22 | 23 | ros::NodeHandle& nh; 24 | 25 | int _vertical_scans; 26 | int _horizontal_scans; 27 | float _scan_period; 28 | float _edge_threshold; 29 | float _surf_threshold; 30 | float _nearest_feature_dist_sqr; 31 | int _mapping_frequency_div; 32 | 33 | std::thread _run_thread; 34 | 35 | Channel& _input_channel; 36 | Channel& _output_channel; 37 | 38 | ros::Publisher pubCornerPointsSharp; 39 | ros::Publisher pubCornerPointsLessSharp; 40 | ros::Publisher pubSurfPointsFlat; 41 | ros::Publisher pubSurfPointsLessFlat; 42 | 43 | pcl::PointCloud::Ptr segmentedCloud; 44 | pcl::PointCloud::Ptr outlierCloud; 45 | 46 | pcl::PointCloud::Ptr cornerPointsSharp; 47 | pcl::PointCloud::Ptr cornerPointsLessSharp; 48 | pcl::PointCloud::Ptr surfPointsFlat; 49 | pcl::PointCloud::Ptr surfPointsLessFlat; 50 | 51 | pcl::PointCloud::Ptr surfPointsLessFlatScan; 52 | pcl::PointCloud::Ptr surfPointsLessFlatScanDS; 53 | 54 | pcl::VoxelGrid downSizeFilter; 55 | 56 | double timeScanCur; 57 | 58 | cloud_msgs::cloud_info segInfo; 59 | std_msgs::Header cloudHeader; 60 | 61 | int systemInitCount; 62 | bool systemInited; 63 | 64 | std::vector cloudSmoothness; 65 | std::vector cloudCurvature; 66 | std::vector cloudNeighborPicked; 67 | std::vector cloudLabel; 68 | 69 | ros::Publisher _pub_cloud_corner_last; 70 | ros::Publisher _pub_cloud_surf_last; 71 | ros::Publisher pubLaserOdometry; 72 | ros::Publisher _pub_outlier_cloudLast; 73 | 74 | int skipFrameNum; 75 | bool systemInitedLM; 76 | 77 | int laserCloudCornerLastNum; 78 | int laserCloudSurfLastNum; 79 | 80 | std::vector pointSelCornerInd; 81 | std::vector pointSearchCornerInd1; 82 | std::vector pointSearchCornerInd2; 83 | 84 | std::vector pointSelSurfInd; 85 | std::vector pointSearchSurfInd1; 86 | std::vector pointSearchSurfInd2; 87 | std::vector pointSearchSurfInd3; 88 | 89 | float transformCur[6]; 90 | float transformSum[6]; 91 | 92 | pcl::PointCloud::Ptr laserCloudCornerLast; 93 | pcl::PointCloud::Ptr laserCloudSurfLast; 94 | pcl::PointCloud::Ptr laserCloudOri; 95 | pcl::PointCloud::Ptr coeffSel; 96 | 97 | nanoflann::KdTreeFLANN kdtreeCornerLast; 98 | nanoflann::KdTreeFLANN kdtreeSurfLast; 99 | 100 | std::vector pointSearchInd; 101 | std::vector pointSearchSqDis; 102 | 103 | nav_msgs::Odometry laserOdometry; 104 | 105 | tf::TransformBroadcaster tfBroadcaster; 106 | tf::StampedTransform laserOdometryTrans; 107 | 108 | bool isDegenerate; 109 | 110 | int frameCount; 111 | size_t _cycle_count; 112 | 113 | private: 114 | void initializationValue(); 115 | void adjustDistortion(); 116 | void calculateSmoothness(); 117 | void markOccludedPoints(); 118 | void extractFeatures(); 119 | 120 | void TransformToStart(PointType const *const pi, PointType *const po); 121 | void TransformToEnd(PointType const *const pi, PointType *const po); 122 | 123 | void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, 124 | float lz, float &ox, float &oy, float &oz); 125 | 126 | void findCorrespondingCornerFeatures(int iterCount); 127 | void findCorrespondingSurfFeatures(int iterCount); 128 | 129 | bool calculateTransformationSurf(int iterCount); 130 | bool calculateTransformationCorner(int iterCount); 131 | bool calculateTransformation(int iterCount); 132 | 133 | void checkSystemInitialization(); 134 | void updateTransformation(); 135 | 136 | void integrateTransformation(); 137 | void publishCloud(); 138 | void publishOdometry(); 139 | 140 | void adjustOutlierCloud(); 141 | void publishCloudsLast(); 142 | 143 | }; 144 | 145 | #endif // FEATUREASSOCIATION_H 146 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/imageProjection.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 | #include 30 | #include "imageProjection.h" 31 | 32 | ImageProjection::ImageProjection(ros::NodeHandle& nh, 33 | Channel& output_channel) 34 | : _nh(nh), 35 | _output_channel(output_channel) 36 | { 37 | _sub_laser_cloud = nh.subscribe( 38 | "/lidar_points", 1, &ImageProjection::cloudHandler, this); 39 | 40 | _pub_full_cloud = 41 | nh.advertise("/full_cloud_projected", 1); 42 | _pub_full_info_cloud = 43 | nh.advertise("/full_cloud_info", 1); 44 | 45 | _pub_ground_cloud = nh.advertise("/ground_cloud", 1); 46 | _pub_segmented_cloud = 47 | nh.advertise("/segmented_cloud", 1); 48 | _pub_segmented_cloud_pure = 49 | nh.advertise("/segmented_cloud_pure", 1); 50 | _pub_segmented_cloud_info = 51 | nh.advertise("/segmented_cloud_info", 1); 52 | _pub_outlier_cloud = nh.advertise("/outlier_cloud", 1); 53 | 54 | nh.getParam("/lego_loam/laser/num_vertical_scans", _vertical_scans); 55 | nh.getParam("/lego_loam/laser/num_horizontal_scans", _horizontal_scans); 56 | nh.getParam("/lego_loam/laser/vertical_angle_bottom", _ang_bottom); 57 | float vertical_angle_top; 58 | nh.getParam("/lego_loam/laser/vertical_angle_top", vertical_angle_top); 59 | 60 | _ang_resolution_X = (M_PI*2) / (_horizontal_scans); 61 | _ang_resolution_Y = DEG_TO_RAD*(vertical_angle_top - _ang_bottom) / float(_vertical_scans-1); 62 | _ang_bottom = -( _ang_bottom - 0.1) * DEG_TO_RAD; 63 | _segment_alpha_X = _ang_resolution_X; 64 | _segment_alpha_Y = _ang_resolution_Y; 65 | 66 | nh.getParam("/lego_loam/imageProjection/segment_theta", _segment_theta); 67 | _segment_theta *= DEG_TO_RAD; 68 | 69 | nh.getParam("/lego_loam/imageProjection/segment_valid_point_num", 70 | _segment_valid_point_num); 71 | nh.getParam("/lego_loam/imageProjection/segment_valid_line_num", 72 | _segment_valid_line_num); 73 | 74 | nh.getParam("/lego_loam/laser/ground_scan_index", 75 | _ground_scan_index); 76 | 77 | nh.getParam("/lego_loam/laser/sensor_mount_angle", 78 | _sensor_mount_angle); 79 | _sensor_mount_angle *= DEG_TO_RAD; 80 | 81 | const size_t cloud_size = _vertical_scans * _horizontal_scans; 82 | 83 | _laser_cloud_in.reset(new pcl::PointCloud()); 84 | _full_cloud.reset(new pcl::PointCloud()); 85 | _full_info_cloud.reset(new pcl::PointCloud()); 86 | 87 | _ground_cloud.reset(new pcl::PointCloud()); 88 | _segmented_cloud.reset(new pcl::PointCloud()); 89 | _segmented_cloud_pure.reset(new pcl::PointCloud()); 90 | _outlier_cloud.reset(new pcl::PointCloud()); 91 | 92 | _full_cloud->points.resize(cloud_size); 93 | _full_info_cloud->points.resize(cloud_size); 94 | 95 | } 96 | 97 | void ImageProjection::resetParameters() { 98 | const size_t cloud_size = _vertical_scans * _horizontal_scans; 99 | PointType nanPoint; 100 | nanPoint.x = std::numeric_limits::quiet_NaN(); 101 | nanPoint.y = std::numeric_limits::quiet_NaN(); 102 | nanPoint.z = std::numeric_limits::quiet_NaN(); 103 | 104 | _laser_cloud_in->clear(); 105 | _ground_cloud->clear(); 106 | _segmented_cloud->clear(); 107 | _segmented_cloud_pure->clear(); 108 | _outlier_cloud->clear(); 109 | 110 | _range_mat.resize(_vertical_scans, _horizontal_scans); 111 | _ground_mat.resize(_vertical_scans, _horizontal_scans); 112 | _label_mat.resize(_vertical_scans, _horizontal_scans); 113 | 114 | _range_mat.fill(FLT_MAX); 115 | _ground_mat.setZero(); 116 | _label_mat.setZero(); 117 | 118 | _label_count = 1; 119 | 120 | std::fill(_full_cloud->points.begin(), _full_cloud->points.end(), nanPoint); 121 | std::fill(_full_info_cloud->points.begin(), _full_info_cloud->points.end(), 122 | nanPoint); 123 | 124 | _seg_msg.startRingIndex.assign(_vertical_scans, 0); 125 | _seg_msg.endRingIndex.assign(_vertical_scans, 0); 126 | 127 | _seg_msg.segmentedCloudGroundFlag.assign(cloud_size, false); 128 | _seg_msg.segmentedCloudColInd.assign(cloud_size, 0); 129 | _seg_msg.segmentedCloudRange.assign(cloud_size, 0); 130 | } 131 | 132 | void ImageProjection::cloudHandler( 133 | const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) { 134 | // Reset parameters 135 | resetParameters(); 136 | 137 | // Copy and remove NAN points 138 | pcl::fromROSMsg(*laserCloudMsg, *_laser_cloud_in); 139 | std::vector indices; 140 | pcl::removeNaNFromPointCloud(*_laser_cloud_in, *_laser_cloud_in, indices); 141 | _seg_msg.header = laserCloudMsg->header; 142 | 143 | findStartEndAngle(); 144 | // Range image projection 145 | projectPointCloud(); 146 | // Mark ground points 147 | groundRemoval(); 148 | // Point cloud segmentation 149 | cloudSegmentation(); 150 | //publish (optionally) 151 | publishClouds(); 152 | } 153 | 154 | 155 | void ImageProjection::projectPointCloud() { 156 | // range image projection 157 | const size_t cloudSize = _laser_cloud_in->points.size(); 158 | 159 | for (size_t i = 0; i < cloudSize; ++i) { 160 | PointType thisPoint = _laser_cloud_in->points[i]; 161 | 162 | float range = sqrt(thisPoint.x * thisPoint.x + 163 | thisPoint.y * thisPoint.y + 164 | thisPoint.z * thisPoint.z); 165 | 166 | // find the row and column index in the image for this point 167 | float verticalAngle = std::asin(thisPoint.z / range); 168 | //std::atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)); 169 | 170 | int rowIdn = (verticalAngle + _ang_bottom) / _ang_resolution_Y; 171 | if (rowIdn < 0 || rowIdn >= _vertical_scans) { 172 | continue; 173 | } 174 | 175 | float horizonAngle = std::atan2(thisPoint.x, thisPoint.y); 176 | 177 | int columnIdn = -round((horizonAngle - M_PI_2) / _ang_resolution_X) + _horizontal_scans * 0.5; 178 | 179 | if (columnIdn >= _horizontal_scans){ 180 | columnIdn -= _horizontal_scans; 181 | } 182 | 183 | if (columnIdn < 0 || columnIdn >= _horizontal_scans){ 184 | continue; 185 | } 186 | 187 | if (range < 0.1){ 188 | continue; 189 | } 190 | 191 | _range_mat(rowIdn, columnIdn) = range; 192 | 193 | thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0; 194 | 195 | size_t index = columnIdn + rowIdn * _horizontal_scans; 196 | _full_cloud->points[index] = thisPoint; 197 | // the corresponding range of a point is saved as "intensity" 198 | _full_info_cloud->points[index] = thisPoint; 199 | _full_info_cloud->points[index].intensity = range; 200 | } 201 | } 202 | 203 | void ImageProjection::findStartEndAngle() { 204 | // start and end orientation of this cloud 205 | auto point = _laser_cloud_in->points.front(); 206 | _seg_msg.startOrientation = -std::atan2(point.y, point.x); 207 | 208 | point = _laser_cloud_in->points.back(); 209 | _seg_msg.endOrientation = -std::atan2(point.y, point.x) + 2 * M_PI; 210 | 211 | if (_seg_msg.endOrientation - _seg_msg.startOrientation > 3 * M_PI) { 212 | _seg_msg.endOrientation -= 2 * M_PI; 213 | } else if (_seg_msg.endOrientation - _seg_msg.startOrientation < M_PI) { 214 | _seg_msg.endOrientation += 2 * M_PI; 215 | } 216 | _seg_msg.orientationDiff = 217 | _seg_msg.endOrientation - _seg_msg.startOrientation; 218 | } 219 | 220 | void ImageProjection::groundRemoval() { 221 | // _ground_mat 222 | // -1, no valid info to check if ground of not 223 | // 0, initial value, after validation, means not ground 224 | // 1, ground 225 | for (size_t j = 0; j < _horizontal_scans; ++j) { 226 | for (size_t i = 0; i < _ground_scan_index; ++i) { 227 | size_t lowerInd = j + (i)*_horizontal_scans; 228 | size_t upperInd = j + (i + 1) * _horizontal_scans; 229 | 230 | if (_full_cloud->points[lowerInd].intensity == -1 || 231 | _full_cloud->points[upperInd].intensity == -1) { 232 | // no info to check, invalid points 233 | _ground_mat(i, j) = -1; 234 | continue; 235 | } 236 | 237 | float dX = 238 | _full_cloud->points[upperInd].x - _full_cloud->points[lowerInd].x; 239 | float dY = 240 | _full_cloud->points[upperInd].y - _full_cloud->points[lowerInd].y; 241 | float dZ = 242 | _full_cloud->points[upperInd].z - _full_cloud->points[lowerInd].z; 243 | 244 | float vertical_angle = std::atan2(dZ , sqrt(dX * dX + dY * dY + dZ * dZ)); 245 | 246 | // TODO: review this change 247 | 248 | if ( (vertical_angle - _sensor_mount_angle) <= 10 * DEG_TO_RAD) { 249 | _ground_mat(i, j) = 1; 250 | _ground_mat(i + 1, j) = 1; 251 | } 252 | } 253 | } 254 | // extract ground cloud (_ground_mat == 1) 255 | // mark entry that doesn't need to label (ground and invalid point) for 256 | // segmentation note that ground remove is from 0~_N_scan-1, need _range_mat 257 | // for mark label matrix for the 16th scan 258 | for (size_t i = 0; i < _vertical_scans; ++i) { 259 | for (size_t j = 0; j < _horizontal_scans; ++j) { 260 | if (_ground_mat(i, j) == 1 || 261 | _range_mat(i, j) == FLT_MAX) { 262 | _label_mat(i, j) = -1; 263 | } 264 | } 265 | } 266 | 267 | for (size_t i = 0; i <= _ground_scan_index; ++i) { 268 | for (size_t j = 0; j < _horizontal_scans; ++j) { 269 | if (_ground_mat(i, j) == 1) 270 | _ground_cloud->push_back(_full_cloud->points[j + i * _horizontal_scans]); 271 | } 272 | } 273 | } 274 | 275 | void ImageProjection::cloudSegmentation() { 276 | // segmentation process 277 | for (size_t i = 0; i < _vertical_scans; ++i) 278 | for (size_t j = 0; j < _horizontal_scans; ++j) 279 | if (_label_mat(i, j) == 0) labelComponents(i, j); 280 | 281 | int sizeOfSegCloud = 0; 282 | // extract segmented cloud for lidar odometry 283 | for (size_t i = 0; i < _vertical_scans; ++i) { 284 | _seg_msg.startRingIndex[i] = sizeOfSegCloud - 1 + 5; 285 | 286 | for (size_t j = 0; j < _horizontal_scans; ++j) { 287 | if (_label_mat(i, j) > 0 || _ground_mat(i, j) == 1) { 288 | // outliers that will not be used for optimization (always continue) 289 | if (_label_mat(i, j) == 999999) { 290 | if (i > _ground_scan_index && j % 5 == 0) { 291 | _outlier_cloud->push_back( 292 | _full_cloud->points[j + i * _horizontal_scans]); 293 | continue; 294 | } else { 295 | continue; 296 | } 297 | } 298 | // majority of ground points are skipped 299 | if (_ground_mat(i, j) == 1) { 300 | if (j % 5 != 0 && j > 5 && j < _horizontal_scans - 5) continue; 301 | } 302 | // mark ground points so they will not be considered as edge features 303 | // later 304 | _seg_msg.segmentedCloudGroundFlag[sizeOfSegCloud] = 305 | (_ground_mat(i, j) == 1); 306 | // mark the points' column index for marking occlusion later 307 | _seg_msg.segmentedCloudColInd[sizeOfSegCloud] = j; 308 | // save range info 309 | _seg_msg.segmentedCloudRange[sizeOfSegCloud] = 310 | _range_mat(i, j); 311 | // save seg cloud 312 | _segmented_cloud->push_back(_full_cloud->points[j + i * _horizontal_scans]); 313 | // size of seg cloud 314 | ++sizeOfSegCloud; 315 | } 316 | } 317 | 318 | _seg_msg.endRingIndex[i] = sizeOfSegCloud - 1 - 5; 319 | } 320 | 321 | // extract segmented cloud for visualization 322 | for (size_t i = 0; i < _vertical_scans; ++i) { 323 | for (size_t j = 0; j < _horizontal_scans; ++j) { 324 | if (_label_mat(i, j) > 0 && _label_mat(i, j) != 999999) { 325 | _segmented_cloud_pure->push_back( 326 | _full_cloud->points[j + i * _horizontal_scans]); 327 | _segmented_cloud_pure->points.back().intensity = 328 | _label_mat(i, j); 329 | } 330 | } 331 | } 332 | } 333 | 334 | void ImageProjection::labelComponents(int row, int col) { 335 | 336 | const float segmentThetaThreshold = tan(_segment_theta); 337 | 338 | std::vector lineCountFlag(_vertical_scans, false); 339 | const size_t cloud_size = _vertical_scans * _horizontal_scans; 340 | using Coord2D = Eigen::Vector2i; 341 | boost::circular_buffer queue(cloud_size); 342 | boost::circular_buffer all_pushed(cloud_size); 343 | 344 | queue.push_back({ row,col } ); 345 | all_pushed.push_back({ row,col } ); 346 | 347 | const Coord2D neighborIterator[4] = { 348 | {0, -1}, {-1, 0}, {1, 0}, {0, 1}}; 349 | 350 | while (queue.size() > 0) { 351 | // Pop point 352 | Coord2D fromInd = queue.front(); 353 | queue.pop_front(); 354 | 355 | // Mark popped point 356 | _label_mat(fromInd.x(), fromInd.y()) = _label_count; 357 | // Loop through all the neighboring grids of popped grid 358 | 359 | for (const auto& iter : neighborIterator) { 360 | // new index 361 | int thisIndX = fromInd.x() + iter.x(); 362 | int thisIndY = fromInd.y() + iter.y(); 363 | // index should be within the boundary 364 | if (thisIndX < 0 || thisIndX >= _vertical_scans){ 365 | continue; 366 | } 367 | // at range image margin (left or right side) 368 | if (thisIndY < 0){ 369 | thisIndY = _horizontal_scans - 1; 370 | } 371 | if (thisIndY >= _horizontal_scans){ 372 | thisIndY = 0; 373 | } 374 | // prevent infinite loop (caused by put already examined point back) 375 | if (_label_mat(thisIndX, thisIndY) != 0){ 376 | continue; 377 | } 378 | 379 | float d1 = std::max(_range_mat(fromInd.x(), fromInd.y()), 380 | _range_mat(thisIndX, thisIndY)); 381 | float d2 = std::min(_range_mat(fromInd.x(), fromInd.y()), 382 | _range_mat(thisIndX, thisIndY)); 383 | 384 | float alpha = (iter.x() == 0) ? _ang_resolution_X : _ang_resolution_Y; 385 | float tang = (d2 * sin(alpha) / (d1 - d2 * cos(alpha))); 386 | 387 | if (tang > segmentThetaThreshold) { 388 | queue.push_back( {thisIndX, thisIndY } ); 389 | 390 | _label_mat(thisIndX, thisIndY) = _label_count; 391 | lineCountFlag[thisIndX] = true; 392 | 393 | all_pushed.push_back( {thisIndX, thisIndY } ); 394 | } 395 | } 396 | } 397 | 398 | // check if this segment is valid 399 | bool feasibleSegment = false; 400 | if (all_pushed.size() >= 30){ 401 | feasibleSegment = true; 402 | } 403 | else if (all_pushed.size() >= _segment_valid_point_num) { 404 | int lineCount = 0; 405 | for (size_t i = 0; i < _vertical_scans; ++i) { 406 | if (lineCountFlag[i] == true) ++lineCount; 407 | } 408 | if (lineCount >= _segment_valid_line_num) feasibleSegment = true; 409 | } 410 | // segment is valid, mark these points 411 | if (feasibleSegment == true) { 412 | ++_label_count; 413 | } else { // segment is invalid, mark these points 414 | for (size_t i = 0; i < all_pushed.size(); ++i) { 415 | _label_mat(all_pushed[i].x(), all_pushed[i].y()) = 999999; 416 | } 417 | } 418 | } 419 | 420 | void ImageProjection::publishClouds() { 421 | 422 | sensor_msgs::PointCloud2 temp; 423 | temp.header.stamp = _seg_msg.header.stamp; 424 | temp.header.frame_id = "base_link"; 425 | 426 | auto PublishCloud = [](ros::Publisher& pub, sensor_msgs::PointCloud2& temp, 427 | const pcl::PointCloud::Ptr& cloud) { 428 | if (pub.getNumSubscribers() != 0) { 429 | pcl::toROSMsg(*cloud, temp); 430 | pub.publish(temp); 431 | } 432 | }; 433 | 434 | PublishCloud(_pub_outlier_cloud, temp, _outlier_cloud); 435 | PublishCloud(_pub_segmented_cloud, temp, _segmented_cloud); 436 | PublishCloud(_pub_full_cloud, temp, _full_cloud); 437 | PublishCloud(_pub_ground_cloud, temp, _ground_cloud); 438 | PublishCloud(_pub_segmented_cloud_pure, temp, _segmented_cloud_pure); 439 | PublishCloud(_pub_full_info_cloud, temp, _full_info_cloud); 440 | 441 | if (_pub_segmented_cloud_info.getNumSubscribers() != 0) { 442 | _pub_segmented_cloud_info.publish(_seg_msg); 443 | } 444 | 445 | //-------------------- 446 | ProjectionOut out; 447 | out.outlier_cloud.reset(new pcl::PointCloud()); 448 | out.segmented_cloud.reset(new pcl::PointCloud()); 449 | 450 | std::swap( out.seg_msg, _seg_msg); 451 | std::swap(out.outlier_cloud, _outlier_cloud); 452 | std::swap(out.segmented_cloud, _segmented_cloud); 453 | 454 | _output_channel.send( std::move(out) ); 455 | 456 | } 457 | 458 | 459 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/imageProjection.h: -------------------------------------------------------------------------------- 1 | #ifndef IMAGEPROJECTION_H 2 | #define IMAGEPROJECTION_H 3 | 4 | #include "lego_loam/utility.h" 5 | #include "lego_loam/channel.h" 6 | #include 7 | 8 | class ImageProjection { 9 | public: 10 | 11 | ImageProjection(ros::NodeHandle& nh, 12 | Channel& output_channel); 13 | 14 | ~ImageProjection() = default; 15 | 16 | void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg); 17 | 18 | private: 19 | void findStartEndAngle(); 20 | void resetParameters(); 21 | void projectPointCloud(); 22 | void groundRemoval(); 23 | void cloudSegmentation(); 24 | void labelComponents(int row, int col); 25 | void publishClouds(); 26 | 27 | pcl::PointCloud::Ptr _laser_cloud_in; 28 | 29 | pcl::PointCloud::Ptr _full_cloud; 30 | pcl::PointCloud::Ptr _full_info_cloud; 31 | 32 | pcl::PointCloud::Ptr _ground_cloud; 33 | pcl::PointCloud::Ptr _segmented_cloud; 34 | pcl::PointCloud::Ptr _segmented_cloud_pure; 35 | pcl::PointCloud::Ptr _outlier_cloud; 36 | 37 | ros::NodeHandle& _nh; 38 | int _vertical_scans; 39 | int _horizontal_scans; 40 | float _ang_bottom; 41 | float _ang_resolution_X; 42 | float _ang_resolution_Y; 43 | float _segment_alpha_X; 44 | float _segment_alpha_Y; 45 | float _segment_theta; 46 | int _segment_valid_point_num; 47 | int _segment_valid_line_num; 48 | int _ground_scan_index; 49 | float _sensor_mount_angle; 50 | 51 | Channel& _output_channel; 52 | 53 | ros::Subscriber _sub_laser_cloud; 54 | 55 | ros::Publisher _pub_full_cloud; 56 | ros::Publisher _pub_full_info_cloud; 57 | 58 | ros::Publisher _pub_ground_cloud; 59 | ros::Publisher _pub_segmented_cloud; 60 | ros::Publisher _pub_segmented_cloud_pure; 61 | ros::Publisher _pub_segmented_cloud_info; 62 | ros::Publisher _pub_outlier_cloud; 63 | 64 | cloud_msgs::cloud_info _seg_msg; 65 | 66 | int _label_count; 67 | 68 | Eigen::MatrixXf _range_mat; // range matrix for range image 69 | Eigen::MatrixXi _label_mat; // label matrix for segmentaiton marking 70 | Eigen::Matrix _ground_mat; // ground matrix for ground cloud marking 71 | 72 | 73 | }; 74 | 75 | 76 | 77 | #endif // IMAGEPROJECTION_H 78 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "featureAssociation.h" 2 | #include "imageProjection.h" 3 | #include "mapOptimization.h" 4 | #include "transformFusion.h" 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | int main(int argc, char** argv) { 12 | ros::init(argc, argv, "lego_loam"); 13 | 14 | ros::NodeHandle nh("~"); 15 | std::string rosbag; 16 | std::string imu_topic; 17 | std::string lidar_topic; 18 | 19 | nh.getParam("rosbag", rosbag); 20 | nh.getParam("imu_topic", imu_topic); 21 | nh.getParam("lidar_topic", lidar_topic); 22 | 23 | bool use_rosbag = false; 24 | 25 | rosbag::Bag bag; 26 | 27 | if (!rosbag.empty()) { 28 | try { 29 | bag.open(rosbag, rosbag::bagmode::Read); 30 | use_rosbag = true; 31 | } catch (std::exception& ex) { 32 | ROS_FATAL("Unable to open rosbag [%s]", rosbag.c_str()); 33 | return 1; 34 | } 35 | } 36 | 37 | Channel projection_out_channel(true); 38 | Channel association_out_channel(use_rosbag); 39 | 40 | ImageProjection IP(nh, projection_out_channel); 41 | 42 | FeatureAssociation FA(nh, projection_out_channel, 43 | association_out_channel); 44 | 45 | MapOptimization MO(nh, association_out_channel); 46 | 47 | TransformFusion TF(nh); 48 | 49 | ROS_INFO("\033[1;32m---->\033[0m LeGO-LOAM Started."); 50 | 51 | if( !use_rosbag ){ 52 | ROS_INFO("SPINNER"); 53 | ros::MultiThreadedSpinner spinner(4); // Use 4 threads 54 | spinner.spin(); 55 | } 56 | else{ 57 | ROS_INFO("ROSBAG"); 58 | std::vector topics; 59 | topics.push_back(imu_topic); 60 | topics.push_back(lidar_topic); 61 | 62 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 63 | 64 | auto start_real_time = std::chrono::high_resolution_clock::now(); 65 | auto start_sim_time = view.getBeginTime(); 66 | 67 | auto prev_real_time = start_real_time; 68 | auto prev_sim_time = start_sim_time; 69 | 70 | auto clock_publisher = nh.advertise("/clock",1); 71 | 72 | for(const rosbag::MessageInstance& m: view) 73 | { 74 | const sensor_msgs::PointCloud2ConstPtr &cloud = m.instantiate(); 75 | if (cloud != NULL){ 76 | IP.cloudHandler(cloud); 77 | //ROS_INFO("cloud"); 78 | } 79 | 80 | rosgraph_msgs::Clock clock_msg; 81 | clock_msg.clock = m.getTime(); 82 | clock_publisher.publish( clock_msg ); 83 | 84 | auto real_time = std::chrono::high_resolution_clock::now(); 85 | if( real_time - prev_real_time > std::chrono::seconds(5) ) 86 | { 87 | auto sim_time = m.getTime(); 88 | auto delta_real = std::chrono::duration_cast(real_time-prev_real_time).count()*0.001; 89 | auto delta_sim = (sim_time - prev_sim_time).toSec(); 90 | ROS_INFO("Processing the rosbag at %.1fX speed.", delta_sim / delta_real); 91 | prev_sim_time = sim_time; 92 | prev_real_time = real_time; 93 | } 94 | ros::spinOnce(); 95 | } 96 | 97 | bag.close(); 98 | 99 | auto real_time = std::chrono::high_resolution_clock::now(); 100 | auto delta_real = std::chrono::duration_cast(real_time-start_real_time).count()*0.001; 101 | auto delta_sim = (view.getEndTime() - start_sim_time).toSec(); 102 | ROS_INFO("Entire rosbag processed at %.1fX speed", delta_sim / delta_real); 103 | } 104 | 105 | 106 | // must be called to cleanup threads 107 | ros::shutdown(); 108 | 109 | return 0; 110 | } 111 | 112 | 113 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/mapOptimization.h: -------------------------------------------------------------------------------- 1 | #ifndef MAPOPTIMIZATION_H 2 | #define MAPOPTIMIZATION_H 3 | 4 | #include "lego_loam/utility.h" 5 | #include "lego_loam/channel.h" 6 | #include "lego_loam/nanoflann_pcl.h" 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | inline gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint) { 19 | // camera frame to lidar frame 20 | return gtsam::Pose3( 21 | gtsam::Rot3::RzRyRx(double(thisPoint.yaw), double(thisPoint.roll), 22 | double(thisPoint.pitch)), 23 | gtsam::Point3(double(thisPoint.z), double(thisPoint.x), 24 | double(thisPoint.y))); 25 | } 26 | 27 | inline Eigen::Affine3f pclPointToAffine3fCameraToLidar( 28 | PointTypePose thisPoint) { 29 | // camera frame to lidar frame 30 | return pcl::getTransformation(thisPoint.z, thisPoint.x, thisPoint.y, 31 | thisPoint.yaw, thisPoint.roll, thisPoint.pitch); 32 | } 33 | 34 | 35 | class MapOptimization { 36 | 37 | public: 38 | MapOptimization(ros::NodeHandle& node, Channel &input_channel); 39 | 40 | ~MapOptimization(); 41 | 42 | void run(); 43 | 44 | private: 45 | gtsam::NonlinearFactorGraph gtSAMgraph; 46 | gtsam::Values initialEstimate; 47 | gtsam::Values optimizedEstimate; 48 | gtsam::ISAM2 *isam; 49 | gtsam::Values isamCurrentEstimate; 50 | 51 | ros::NodeHandle& nh; 52 | bool _loop_closure_enabled; 53 | float _scan_period; 54 | 55 | float _surrounding_keyframe_search_radius; 56 | int _surrounding_keyframe_search_num; 57 | float _history_keyframe_search_radius; 58 | int _history_keyframe_search_num; 59 | float _history_keyframe_fitness_score; 60 | float _global_map_visualization_search_radius; 61 | 62 | Channel& _input_channel; 63 | std::thread _run_thread; 64 | 65 | Channel _publish_global_signal; 66 | std::thread _publish_global_thread; 67 | void publishGlobalMapThread(); 68 | 69 | Channel _loop_closure_signal; 70 | std::thread _loop_closure_thread; 71 | void loopClosureThread(); 72 | 73 | ros::Publisher pubLaserCloudSurround; 74 | ros::Publisher pubOdomAftMapped; 75 | ros::Publisher pubKeyPoses; 76 | 77 | ros::Publisher pubHistoryKeyFrames; 78 | ros::Publisher pubIcpKeyFrames; 79 | ros::Publisher pubRecentKeyFrames; 80 | 81 | nav_msgs::Odometry odomAftMapped; 82 | tf::StampedTransform aftMappedTrans; 83 | tf::TransformBroadcaster tfBroadcaster; 84 | 85 | std::vector::Ptr> cornerCloudKeyFrames; 86 | std::vector::Ptr> surfCloudKeyFrames; 87 | std::vector::Ptr> outlierCloudKeyFrames; 88 | 89 | std::deque::Ptr> recentCornerCloudKeyFrames; 90 | std::deque::Ptr> recentSurfCloudKeyFrames; 91 | std::deque::Ptr> recentOutlierCloudKeyFrames; 92 | int latestFrameID; 93 | 94 | std::vector surroundingExistingKeyPosesID; 95 | std::deque::Ptr> surroundingCornerCloudKeyFrames; 96 | std::deque::Ptr> surroundingSurfCloudKeyFrames; 97 | std::deque::Ptr> surroundingOutlierCloudKeyFrames; 98 | 99 | PointType previousRobotPosPoint; 100 | PointType currentRobotPosPoint; 101 | 102 | pcl::PointCloud::Ptr cloudKeyPoses3D; 103 | pcl::PointCloud::Ptr cloudKeyPoses6D; 104 | 105 | pcl::PointCloud::Ptr surroundingKeyPoses; 106 | pcl::PointCloud::Ptr surroundingKeyPosesDS; 107 | 108 | pcl::PointCloud::Ptr 109 | laserCloudCornerLast; // corner feature set from odoOptimization 110 | pcl::PointCloud::Ptr 111 | laserCloudSurfLast; // surf feature set from odoOptimization 112 | pcl::PointCloud::Ptr 113 | laserCloudCornerLastDS; // downsampled corner featuer set from 114 | // odoOptimization 115 | pcl::PointCloud::Ptr 116 | laserCloudSurfLastDS; // downsampled surf featuer set from 117 | // odoOptimization 118 | 119 | pcl::PointCloud::Ptr 120 | laserCloudOutlierLast; // corner feature set from odoOptimization 121 | pcl::PointCloud::Ptr 122 | laserCloudOutlierLastDS; // corner feature set from odoOptimization 123 | 124 | pcl::PointCloud::Ptr 125 | laserCloudSurfTotalLast; // surf feature set from odoOptimization 126 | pcl::PointCloud::Ptr 127 | laserCloudSurfTotalLastDS; // downsampled corner featuer set from 128 | // odoOptimization 129 | 130 | pcl::PointCloud::Ptr laserCloudOri; 131 | pcl::PointCloud::Ptr coeffSel; 132 | 133 | pcl::PointCloud::Ptr laserCloudCornerFromMap; 134 | pcl::PointCloud::Ptr laserCloudSurfFromMap; 135 | pcl::PointCloud::Ptr laserCloudCornerFromMapDS; 136 | pcl::PointCloud::Ptr laserCloudSurfFromMapDS; 137 | 138 | nanoflann::KdTreeFLANN kdtreeCornerFromMap; 139 | nanoflann::KdTreeFLANN kdtreeSurfFromMap; 140 | 141 | nanoflann::KdTreeFLANN kdtreeSurroundingKeyPoses; 142 | nanoflann::KdTreeFLANN kdtreeHistoryKeyPoses; 143 | 144 | pcl::PointCloud::Ptr nearHistoryCornerKeyFrameCloud; 145 | pcl::PointCloud::Ptr nearHistoryCornerKeyFrameCloudDS; 146 | pcl::PointCloud::Ptr nearHistorySurfKeyFrameCloud; 147 | pcl::PointCloud::Ptr nearHistorySurfKeyFrameCloudDS; 148 | 149 | pcl::PointCloud::Ptr latestCornerKeyFrameCloud; 150 | pcl::PointCloud::Ptr latestSurfKeyFrameCloud; 151 | pcl::PointCloud::Ptr latestSurfKeyFrameCloudDS; 152 | 153 | nanoflann::KdTreeFLANN kdtreeGlobalMap; 154 | pcl::PointCloud::Ptr globalMapKeyPoses; 155 | pcl::PointCloud::Ptr globalMapKeyPosesDS; 156 | pcl::PointCloud::Ptr globalMapKeyFrames; 157 | pcl::PointCloud::Ptr globalMapKeyFramesDS; 158 | 159 | std::vector pointSearchInd; 160 | std::vector pointSearchSqDis; 161 | 162 | pcl::VoxelGrid downSizeFilterCorner; 163 | pcl::VoxelGrid downSizeFilterSurf; 164 | pcl::VoxelGrid downSizeFilterOutlier; 165 | pcl::VoxelGrid 166 | downSizeFilterHistoryKeyFrames; // for histor key frames of loop closure 167 | pcl::VoxelGrid 168 | downSizeFilterSurroundingKeyPoses; // for surrounding key poses of 169 | // scan-to-map optimization 170 | pcl::VoxelGrid 171 | downSizeFilterGlobalMapKeyPoses; // for global map visualization 172 | pcl::VoxelGrid 173 | downSizeFilterGlobalMapKeyFrames; // for global map visualization 174 | 175 | double timeLaserOdometry; 176 | double timeLastGloalMapPublish; 177 | 178 | float transformLast[6]; 179 | float transformSum[6]; 180 | float transformIncre[6]; 181 | float transformTobeMapped[6]; 182 | float transformBefMapped[6]; 183 | float transformAftMapped[6]; 184 | 185 | 186 | std::mutex mtx; 187 | 188 | double timeLastProcessing; 189 | 190 | PointType pointOri, pointSel, pointProj, coeff; 191 | 192 | Eigen::Matrix matA0; 193 | Eigen::Matrix matB0; 194 | Eigen::Vector3f matX0; 195 | 196 | Eigen::Matrix3f matA1; 197 | Eigen::Matrix matD1; 198 | Eigen::Matrix3f matV1; 199 | 200 | Eigen::Matrix matP; 201 | 202 | bool isDegenerate; 203 | 204 | int laserCloudCornerFromMapDSNum; 205 | int laserCloudSurfFromMapDSNum; 206 | int laserCloudCornerLastDSNum; 207 | int laserCloudSurfLastDSNum; 208 | int laserCloudOutlierLastDSNum; 209 | int laserCloudSurfTotalLastDSNum; 210 | 211 | bool potentialLoopFlag; 212 | double timeSaveFirstCurrentScanForLoopClosure; 213 | int closestHistoryFrameID; 214 | int latestFrameIDLoopCloure; 215 | 216 | bool aLoopIsClosed; 217 | 218 | float cRoll, sRoll, cPitch, sPitch, cYaw, sYaw, tX, tY, tZ; 219 | float ctRoll, stRoll, ctPitch, stPitch, ctYaw, stYaw, tInX, tInY, tInZ; 220 | 221 | private: 222 | void allocateMemory(); 223 | void transformAssociateToMap(); 224 | void transformUpdate(); 225 | void updatePointAssociateToMapSinCos(); 226 | void pointAssociateToMap(PointType const *const pi, PointType *const po); 227 | void updateTransformPointCloudSinCos(PointTypePose *tIn) ; 228 | 229 | pcl::PointCloud::Ptr transformPointCloud( 230 | pcl::PointCloud::Ptr cloudIn); 231 | 232 | pcl::PointCloud::Ptr transformPointCloud( 233 | pcl::PointCloud::Ptr cloudIn, PointTypePose *transformIn); 234 | 235 | void publishTF(); 236 | void publishKeyPosesAndFrames(); 237 | void publishGlobalMap(); 238 | 239 | bool detectLoopClosure(); 240 | void performLoopClosure(); 241 | 242 | void extractSurroundingKeyFrames(); 243 | void downsampleCurrentScan(); 244 | void cornerOptimization(int iterCount); 245 | void surfOptimization(int iterCount); 246 | 247 | bool LMOptimization(int iterCount); 248 | void scan2MapOptimization(); 249 | 250 | void saveKeyFramesAndFactor(); 251 | void correctPoses(); 252 | 253 | void clearCloud(); 254 | }; 255 | 256 | #endif // MAPOPTIMIZATION_H 257 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/mapOptmization.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 | // T. Shan and B. Englot. LeGO-LOAM: Lightweight and Ground-Optimized Lidar 33 | // Odometry and Mapping on Variable Terrain 34 | // IEEE/RSJ International Conference on Intelligent Robots and Systems 35 | // (IROS). October 2018. 36 | 37 | #include "mapOptimization.h" 38 | #include 39 | 40 | using namespace gtsam; 41 | 42 | MapOptimization::MapOptimization(ros::NodeHandle &node, 43 | Channel &input_channel) 44 | : nh(node), 45 | _input_channel(input_channel), 46 | _publish_global_signal(false), 47 | _loop_closure_signal(false) 48 | { 49 | ISAM2Params parameters; 50 | parameters.relinearizeThreshold = 0.01; 51 | parameters.relinearizeSkip = 1; 52 | isam = new ISAM2(parameters); 53 | 54 | pubKeyPoses = nh.advertise("/key_pose_origin", 2); 55 | pubLaserCloudSurround = 56 | nh.advertise("/laser_cloud_surround", 2); 57 | pubOdomAftMapped = nh.advertise("/aft_mapped_to_init", 5); 58 | 59 | pubHistoryKeyFrames = 60 | nh.advertise("/history_cloud", 2); 61 | pubIcpKeyFrames = 62 | nh.advertise("/corrected_cloud", 2); 63 | pubRecentKeyFrames = 64 | nh.advertise("/recent_cloud", 2); 65 | 66 | downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2); 67 | downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4); 68 | downSizeFilterOutlier.setLeafSize(0.4, 0.4, 0.4); 69 | 70 | // for histor key frames of loop closure 71 | downSizeFilterHistoryKeyFrames.setLeafSize(0.4, 0.4, 0.4); 72 | // for surrounding key poses of scan-to-map optimization 73 | downSizeFilterSurroundingKeyPoses.setLeafSize(1.0, 1.0, 1.0); 74 | 75 | // for global map visualization 76 | downSizeFilterGlobalMapKeyPoses.setLeafSize(1.0, 1.0, 1.0); 77 | // for global map visualization 78 | downSizeFilterGlobalMapKeyFrames.setLeafSize(0.4, 0.4, 0.4); 79 | 80 | odomAftMapped.header.frame_id = "/camera_init"; 81 | odomAftMapped.child_frame_id = "/aft_mapped"; 82 | 83 | aftMappedTrans.frame_id_ = "/camera_init"; 84 | aftMappedTrans.child_frame_id_ = "/aft_mapped"; 85 | 86 | nh.getParam("/lego_loam/laser/scan_period", _scan_period); 87 | 88 | nh.getParam("/lego_loam/mapping/enable_loop_closure", _loop_closure_enabled); 89 | 90 | nh.getParam("/lego_loam/mapping/history_keyframe_search_radius", 91 | _history_keyframe_search_radius); 92 | 93 | nh.getParam("/lego_loam/mapping/history_keyframe_search_num", 94 | _history_keyframe_search_num); 95 | 96 | nh.getParam("/lego_loam/mapping/history_keyframe_fitness_score", 97 | _history_keyframe_fitness_score); 98 | 99 | nh.getParam("/lego_loam/mapping/surrounding_keyframe_search_radius", 100 | _surrounding_keyframe_search_radius); 101 | 102 | nh.getParam("/lego_loam/mapping/surrounding_keyframe_search_num", 103 | _surrounding_keyframe_search_num); 104 | 105 | nh.getParam("/lego_loam/mapping/global_map_visualization_search_radius", 106 | _global_map_visualization_search_radius); 107 | 108 | allocateMemory(); 109 | 110 | _publish_global_thread = std::thread(&MapOptimization::publishGlobalMapThread, this); 111 | _loop_closure_thread = std::thread(&MapOptimization::loopClosureThread, this); 112 | _run_thread = std::thread(&MapOptimization::run, this); 113 | 114 | } 115 | 116 | MapOptimization::~MapOptimization() 117 | { 118 | _input_channel.send({}); 119 | _run_thread.join(); 120 | 121 | _publish_global_signal.send(false); 122 | _publish_global_thread.join(); 123 | 124 | _loop_closure_signal.send(false); 125 | _loop_closure_thread.join(); 126 | } 127 | 128 | void MapOptimization::allocateMemory() { 129 | cloudKeyPoses3D.reset(new pcl::PointCloud()); 130 | cloudKeyPoses6D.reset(new pcl::PointCloud()); 131 | 132 | surroundingKeyPoses.reset(new pcl::PointCloud()); 133 | surroundingKeyPosesDS.reset(new pcl::PointCloud()); 134 | 135 | laserCloudCornerLast.reset( 136 | new pcl::PointCloud()); // corner feature set from 137 | // odoOptimization 138 | laserCloudSurfLast.reset( 139 | new pcl::PointCloud()); // surf feature set from 140 | // odoOptimization 141 | laserCloudCornerLastDS.reset( 142 | new pcl::PointCloud()); // downsampled corner featuer set 143 | // from odoOptimization 144 | laserCloudSurfLastDS.reset( 145 | new pcl::PointCloud()); // downsampled surf featuer set from 146 | // odoOptimization 147 | laserCloudOutlierLast.reset( 148 | new pcl::PointCloud()); // corner feature set from 149 | // odoOptimization 150 | laserCloudOutlierLastDS.reset( 151 | new pcl::PointCloud()); // downsampled corner feature set 152 | // from odoOptimization 153 | laserCloudSurfTotalLast.reset( 154 | new pcl::PointCloud()); // surf feature set from 155 | // odoOptimization 156 | laserCloudSurfTotalLastDS.reset( 157 | new pcl::PointCloud()); // downsampled surf featuer set from 158 | // odoOptimization 159 | 160 | laserCloudOri.reset(new pcl::PointCloud()); 161 | coeffSel.reset(new pcl::PointCloud()); 162 | 163 | laserCloudCornerFromMap.reset(new pcl::PointCloud()); 164 | laserCloudSurfFromMap.reset(new pcl::PointCloud()); 165 | laserCloudCornerFromMapDS.reset(new pcl::PointCloud()); 166 | laserCloudSurfFromMapDS.reset(new pcl::PointCloud()); 167 | 168 | nearHistoryCornerKeyFrameCloud.reset(new pcl::PointCloud()); 169 | nearHistoryCornerKeyFrameCloudDS.reset(new pcl::PointCloud()); 170 | nearHistorySurfKeyFrameCloud.reset(new pcl::PointCloud()); 171 | nearHistorySurfKeyFrameCloudDS.reset(new pcl::PointCloud()); 172 | 173 | latestCornerKeyFrameCloud.reset(new pcl::PointCloud()); 174 | latestSurfKeyFrameCloud.reset(new pcl::PointCloud()); 175 | latestSurfKeyFrameCloudDS.reset(new pcl::PointCloud()); 176 | 177 | globalMapKeyPoses.reset(new pcl::PointCloud()); 178 | globalMapKeyPosesDS.reset(new pcl::PointCloud()); 179 | globalMapKeyFrames.reset(new pcl::PointCloud()); 180 | globalMapKeyFramesDS.reset(new pcl::PointCloud()); 181 | 182 | timeLaserOdometry = 0; 183 | timeLastGloalMapPublish = 0; 184 | timeLastProcessing = -1; 185 | 186 | for (int i = 0; i < 6; ++i) { 187 | transformLast[i] = 0; 188 | transformSum[i] = 0; 189 | transformIncre[i] = 0; 190 | transformTobeMapped[i] = 0; 191 | transformBefMapped[i] = 0; 192 | transformAftMapped[i] = 0; 193 | } 194 | 195 | 196 | matA0.setZero(); 197 | matB0.fill(-1); 198 | matX0.setZero(); 199 | 200 | matA1.setZero(); 201 | matD1.setZero(); 202 | matV1.setZero(); 203 | 204 | isDegenerate = false; 205 | matP.setZero(); 206 | 207 | laserCloudCornerFromMapDSNum = 0; 208 | laserCloudSurfFromMapDSNum = 0; 209 | laserCloudCornerLastDSNum = 0; 210 | laserCloudSurfLastDSNum = 0; 211 | laserCloudOutlierLastDSNum = 0; 212 | laserCloudSurfTotalLastDSNum = 0; 213 | 214 | potentialLoopFlag = false; 215 | aLoopIsClosed = false; 216 | 217 | latestFrameID = 0; 218 | } 219 | 220 | 221 | void MapOptimization::publishGlobalMapThread() 222 | { 223 | while(ros::ok()) 224 | { 225 | bool ready; 226 | _publish_global_signal.receive(ready); 227 | if(ready){ 228 | publishGlobalMap(); 229 | } 230 | } 231 | } 232 | 233 | void MapOptimization::loopClosureThread() 234 | { 235 | while(ros::ok()) 236 | { 237 | bool ready; 238 | _loop_closure_signal.receive(ready); 239 | if(ready && _loop_closure_enabled){ 240 | performLoopClosure(); 241 | } 242 | } 243 | } 244 | 245 | void MapOptimization::transformAssociateToMap() { 246 | float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - 247 | sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 248 | float y1 = transformBefMapped[4] - transformSum[4]; 249 | float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) + 250 | cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 251 | 252 | float x2 = x1; 253 | float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1; 254 | float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1; 255 | 256 | transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2; 257 | transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2; 258 | transformIncre[5] = z2; 259 | 260 | float sbcx = sin(transformSum[0]); 261 | float cbcx = cos(transformSum[0]); 262 | float sbcy = sin(transformSum[1]); 263 | float cbcy = cos(transformSum[1]); 264 | float sbcz = sin(transformSum[2]); 265 | float cbcz = cos(transformSum[2]); 266 | 267 | float sblx = sin(transformBefMapped[0]); 268 | float cblx = cos(transformBefMapped[0]); 269 | float sbly = sin(transformBefMapped[1]); 270 | float cbly = cos(transformBefMapped[1]); 271 | float sblz = sin(transformBefMapped[2]); 272 | float cblz = cos(transformBefMapped[2]); 273 | 274 | float salx = sin(transformAftMapped[0]); 275 | float calx = cos(transformAftMapped[0]); 276 | float saly = sin(transformAftMapped[1]); 277 | float caly = cos(transformAftMapped[1]); 278 | float salz = sin(transformAftMapped[2]); 279 | float calz = cos(transformAftMapped[2]); 280 | 281 | float srx = -sbcx * (salx * sblx + calx * cblx * salz * sblz + 282 | calx * calz * cblx * cblz) - 283 | cbcx * sbcy * 284 | (calx * calz * (cbly * sblz - cblz * sblx * sbly) - 285 | calx * salz * (cbly * cblz + sblx * sbly * sblz) + 286 | cblx * salx * sbly) - 287 | cbcx * cbcy * 288 | (calx * salz * (cblz * sbly - cbly * sblx * sblz) - 289 | calx * calz * (sbly * sblz + cbly * cblz * sblx) + 290 | cblx * cbly * salx); 291 | transformTobeMapped[0] = -asin(srx); 292 | 293 | float srycrx = sbcx * (cblx * cblz * (caly * salz - calz * salx * saly) - 294 | cblx * sblz * (caly * calz + salx * saly * salz) + 295 | calx * saly * sblx) - 296 | cbcx * cbcy * 297 | ((caly * calz + salx * saly * salz) * 298 | (cblz * sbly - cbly * sblx * sblz) + 299 | (caly * salz - calz * salx * saly) * 300 | (sbly * sblz + cbly * cblz * sblx) - 301 | calx * cblx * cbly * saly) + 302 | cbcx * sbcy * 303 | ((caly * calz + salx * saly * salz) * 304 | (cbly * cblz + sblx * sbly * sblz) + 305 | (caly * salz - calz * salx * saly) * 306 | (cbly * sblz - cblz * sblx * sbly) + 307 | calx * cblx * saly * sbly); 308 | float crycrx = sbcx * (cblx * sblz * (calz * saly - caly * salx * salz) - 309 | cblx * cblz * (saly * salz + caly * calz * salx) + 310 | calx * caly * sblx) + 311 | cbcx * cbcy * 312 | ((saly * salz + caly * calz * salx) * 313 | (sbly * sblz + cbly * cblz * sblx) + 314 | (calz * saly - caly * salx * salz) * 315 | (cblz * sbly - cbly * sblx * sblz) + 316 | calx * caly * cblx * cbly) - 317 | cbcx * sbcy * 318 | ((saly * salz + caly * calz * salx) * 319 | (cbly * sblz - cblz * sblx * sbly) + 320 | (calz * saly - caly * salx * salz) * 321 | (cbly * cblz + sblx * sbly * sblz) - 322 | calx * caly * cblx * sbly); 323 | transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]), 324 | crycrx / cos(transformTobeMapped[0])); 325 | 326 | float srzcrx = 327 | (cbcz * sbcy - cbcy * sbcx * sbcz) * 328 | (calx * salz * (cblz * sbly - cbly * sblx * sblz) - 329 | calx * calz * (sbly * sblz + cbly * cblz * sblx) + 330 | cblx * cbly * salx) - 331 | (cbcy * cbcz + sbcx * sbcy * sbcz) * 332 | (calx * calz * (cbly * sblz - cblz * sblx * sbly) - 333 | calx * salz * (cbly * cblz + sblx * sbly * sblz) + 334 | cblx * salx * sbly) + 335 | cbcx * sbcz * 336 | (salx * sblx + calx * cblx * salz * sblz + calx * calz * cblx * cblz); 337 | float crzcrx = 338 | (cbcy * sbcz - cbcz * sbcx * sbcy) * 339 | (calx * calz * (cbly * sblz - cblz * sblx * sbly) - 340 | calx * salz * (cbly * cblz + sblx * sbly * sblz) + 341 | cblx * salx * sbly) - 342 | (sbcy * sbcz + cbcy * cbcz * sbcx) * 343 | (calx * salz * (cblz * sbly - cbly * sblx * sblz) - 344 | calx * calz * (sbly * sblz + cbly * cblz * sblx) + 345 | cblx * cbly * salx) + 346 | cbcx * cbcz * 347 | (salx * sblx + calx * cblx * salz * sblz + calx * calz * cblx * cblz); 348 | transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]), 349 | crzcrx / cos(transformTobeMapped[0])); 350 | 351 | x1 = cos(transformTobeMapped[2]) * transformIncre[3] - 352 | sin(transformTobeMapped[2]) * transformIncre[4]; 353 | y1 = sin(transformTobeMapped[2]) * transformIncre[3] + 354 | cos(transformTobeMapped[2]) * transformIncre[4]; 355 | z1 = transformIncre[5]; 356 | 357 | x2 = x1; 358 | y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1; 359 | z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1; 360 | 361 | transformTobeMapped[3] = 362 | transformAftMapped[3] - 363 | (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2); 364 | transformTobeMapped[4] = transformAftMapped[4] - y2; 365 | transformTobeMapped[5] = 366 | transformAftMapped[5] - 367 | (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2); 368 | } 369 | 370 | void MapOptimization::transformUpdate() { 371 | 372 | for (int i = 0; i < 6; i++) { 373 | transformBefMapped[i] = transformSum[i]; 374 | transformAftMapped[i] = transformTobeMapped[i]; 375 | } 376 | } 377 | 378 | void MapOptimization::updatePointAssociateToMapSinCos() { 379 | cRoll = cos(transformTobeMapped[0]); 380 | sRoll = sin(transformTobeMapped[0]); 381 | 382 | cPitch = cos(transformTobeMapped[1]); 383 | sPitch = sin(transformTobeMapped[1]); 384 | 385 | cYaw = cos(transformTobeMapped[2]); 386 | sYaw = sin(transformTobeMapped[2]); 387 | 388 | tX = transformTobeMapped[3]; 389 | tY = transformTobeMapped[4]; 390 | tZ = transformTobeMapped[5]; 391 | } 392 | 393 | void MapOptimization::pointAssociateToMap(PointType const *const pi, 394 | PointType *const po) { 395 | float x1 = cYaw * pi->x - sYaw * pi->y; 396 | float y1 = sYaw * pi->x + cYaw * pi->y; 397 | float z1 = pi->z; 398 | 399 | float x2 = x1; 400 | float y2 = cRoll * y1 - sRoll * z1; 401 | float z2 = sRoll * y1 + cRoll * z1; 402 | 403 | po->x = cPitch * x2 + sPitch * z2 + tX; 404 | po->y = y2 + tY; 405 | po->z = -sPitch * x2 + cPitch * z2 + tZ; 406 | po->intensity = pi->intensity; 407 | } 408 | 409 | void MapOptimization::updateTransformPointCloudSinCos(PointTypePose *tIn) { 410 | ctRoll = cos(tIn->roll); 411 | stRoll = sin(tIn->roll); 412 | 413 | ctPitch = cos(tIn->pitch); 414 | stPitch = sin(tIn->pitch); 415 | 416 | ctYaw = cos(tIn->yaw); 417 | stYaw = sin(tIn->yaw); 418 | 419 | tInX = tIn->x; 420 | tInY = tIn->y; 421 | tInZ = tIn->z; 422 | } 423 | 424 | pcl::PointCloud::Ptr MapOptimization::transformPointCloud( 425 | pcl::PointCloud::Ptr cloudIn) { 426 | // !!! DO NOT use pcl for point cloud transformation, results are not 427 | // accurate Reason: unkown 428 | pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); 429 | 430 | PointType *pointFrom; 431 | PointType pointTo; 432 | 433 | int cloudSize = cloudIn->points.size(); 434 | cloudOut->resize(cloudSize); 435 | 436 | for (int i = 0; i < cloudSize; ++i) { 437 | pointFrom = &cloudIn->points[i]; 438 | float x1 = ctYaw * pointFrom->x - stYaw * pointFrom->y; 439 | float y1 = stYaw * pointFrom->x + ctYaw * pointFrom->y; 440 | float z1 = pointFrom->z; 441 | 442 | float x2 = x1; 443 | float y2 = ctRoll * y1 - stRoll * z1; 444 | float z2 = stRoll * y1 + ctRoll * z1; 445 | 446 | pointTo.x = ctPitch * x2 + stPitch * z2 + tInX; 447 | pointTo.y = y2 + tInY; 448 | pointTo.z = -stPitch * x2 + ctPitch * z2 + tInZ; 449 | pointTo.intensity = pointFrom->intensity; 450 | 451 | cloudOut->points[i] = pointTo; 452 | } 453 | return cloudOut; 454 | } 455 | 456 | pcl::PointCloud::Ptr MapOptimization::transformPointCloud( 457 | pcl::PointCloud::Ptr cloudIn, PointTypePose *transformIn) { 458 | pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); 459 | 460 | PointType *pointFrom; 461 | PointType pointTo; 462 | 463 | int cloudSize = cloudIn->points.size(); 464 | cloudOut->resize(cloudSize); 465 | 466 | for (int i = 0; i < cloudSize; ++i) { 467 | pointFrom = &cloudIn->points[i]; 468 | float x1 = cos(transformIn->yaw) * pointFrom->x - 469 | sin(transformIn->yaw) * pointFrom->y; 470 | float y1 = sin(transformIn->yaw) * pointFrom->x + 471 | cos(transformIn->yaw) * pointFrom->y; 472 | float z1 = pointFrom->z; 473 | 474 | float x2 = x1; 475 | float y2 = cos(transformIn->roll) * y1 - sin(transformIn->roll) * z1; 476 | float z2 = sin(transformIn->roll) * y1 + cos(transformIn->roll) * z1; 477 | 478 | pointTo.x = cos(transformIn->pitch) * x2 + sin(transformIn->pitch) * z2 + 479 | transformIn->x; 480 | pointTo.y = y2 + transformIn->y; 481 | pointTo.z = -sin(transformIn->pitch) * x2 + cos(transformIn->pitch) * z2 + 482 | transformIn->z; 483 | pointTo.intensity = pointFrom->intensity; 484 | 485 | cloudOut->points[i] = pointTo; 486 | } 487 | return cloudOut; 488 | } 489 | 490 | 491 | void MapOptimization::publishTF() { 492 | geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw( 493 | transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]); 494 | 495 | odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry); 496 | odomAftMapped.pose.pose.orientation.x = -geoQuat.y; 497 | odomAftMapped.pose.pose.orientation.y = -geoQuat.z; 498 | odomAftMapped.pose.pose.orientation.z = geoQuat.x; 499 | odomAftMapped.pose.pose.orientation.w = geoQuat.w; 500 | odomAftMapped.pose.pose.position.x = transformAftMapped[3]; 501 | odomAftMapped.pose.pose.position.y = transformAftMapped[4]; 502 | odomAftMapped.pose.pose.position.z = transformAftMapped[5]; 503 | odomAftMapped.twist.twist.angular.x = transformBefMapped[0]; 504 | odomAftMapped.twist.twist.angular.y = transformBefMapped[1]; 505 | odomAftMapped.twist.twist.angular.z = transformBefMapped[2]; 506 | odomAftMapped.twist.twist.linear.x = transformBefMapped[3]; 507 | odomAftMapped.twist.twist.linear.y = transformBefMapped[4]; 508 | odomAftMapped.twist.twist.linear.z = transformBefMapped[5]; 509 | pubOdomAftMapped.publish(odomAftMapped); 510 | 511 | aftMappedTrans.stamp_ = ros::Time().fromSec(timeLaserOdometry); 512 | aftMappedTrans.setRotation( 513 | tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 514 | aftMappedTrans.setOrigin(tf::Vector3( 515 | transformAftMapped[3], transformAftMapped[4], transformAftMapped[5])); 516 | tfBroadcaster.sendTransform(aftMappedTrans); 517 | } 518 | 519 | void MapOptimization::publishKeyPosesAndFrames() { 520 | if (pubKeyPoses.getNumSubscribers() != 0) { 521 | sensor_msgs::PointCloud2 cloudMsgTemp; 522 | pcl::toROSMsg(*cloudKeyPoses3D, cloudMsgTemp); 523 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 524 | cloudMsgTemp.header.frame_id = "/camera_init"; 525 | pubKeyPoses.publish(cloudMsgTemp); 526 | } 527 | 528 | if (pubRecentKeyFrames.getNumSubscribers() != 0) { 529 | sensor_msgs::PointCloud2 cloudMsgTemp; 530 | pcl::toROSMsg(*laserCloudSurfFromMapDS, cloudMsgTemp); 531 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 532 | cloudMsgTemp.header.frame_id = "/camera_init"; 533 | pubRecentKeyFrames.publish(cloudMsgTemp); 534 | } 535 | } 536 | 537 | void MapOptimization::publishGlobalMap() { 538 | if (pubLaserCloudSurround.getNumSubscribers() == 0) return; 539 | 540 | if (cloudKeyPoses3D->points.empty() == true) return; 541 | // kd-tree to find near key frames to visualize 542 | std::vector pointSearchIndGlobalMap; 543 | std::vector pointSearchSqDisGlobalMap; 544 | // search near key frames to visualize 545 | mtx.lock(); 546 | kdtreeGlobalMap.setInputCloud(cloudKeyPoses3D); 547 | kdtreeGlobalMap.radiusSearch( 548 | currentRobotPosPoint, _global_map_visualization_search_radius, 549 | pointSearchIndGlobalMap, pointSearchSqDisGlobalMap); 550 | mtx.unlock(); 551 | 552 | for (int i = 0; i < pointSearchIndGlobalMap.size(); ++i) 553 | globalMapKeyPoses->points.push_back( 554 | cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]); 555 | // downsample near selected key frames 556 | downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses); 557 | downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS); 558 | // extract visualized and downsampled key frames 559 | for (int i = 0; i < globalMapKeyPosesDS->points.size(); ++i) { 560 | int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity; 561 | *globalMapKeyFrames += *transformPointCloud( 562 | cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]); 563 | *globalMapKeyFrames += *transformPointCloud( 564 | surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]); 565 | *globalMapKeyFrames += 566 | *transformPointCloud(outlierCloudKeyFrames[thisKeyInd], 567 | &cloudKeyPoses6D->points[thisKeyInd]); 568 | } 569 | // downsample visualized points 570 | downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames); 571 | downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS); 572 | 573 | sensor_msgs::PointCloud2 cloudMsgTemp; 574 | pcl::toROSMsg(*globalMapKeyFramesDS, cloudMsgTemp); 575 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 576 | cloudMsgTemp.header.frame_id = "/camera_init"; 577 | pubLaserCloudSurround.publish(cloudMsgTemp); 578 | 579 | globalMapKeyPoses->clear(); 580 | globalMapKeyPosesDS->clear(); 581 | globalMapKeyFrames->clear(); 582 | //globalMapKeyFramesDS->clear(); 583 | } 584 | 585 | bool MapOptimization::detectLoopClosure() { 586 | latestSurfKeyFrameCloud->clear(); 587 | nearHistorySurfKeyFrameCloud->clear(); 588 | nearHistorySurfKeyFrameCloudDS->clear(); 589 | 590 | std::lock_guard lock(mtx); 591 | // find the closest history key frame 592 | std::vector pointSearchIndLoop; 593 | std::vector pointSearchSqDisLoop; 594 | kdtreeHistoryKeyPoses.setInputCloud(cloudKeyPoses3D); 595 | kdtreeHistoryKeyPoses.radiusSearch( 596 | currentRobotPosPoint, _history_keyframe_search_radius, pointSearchIndLoop, 597 | pointSearchSqDisLoop); 598 | 599 | closestHistoryFrameID = -1; 600 | for (int i = 0; i < pointSearchIndLoop.size(); ++i) { 601 | int id = pointSearchIndLoop[i]; 602 | if (abs(cloudKeyPoses6D->points[id].time - timeLaserOdometry) > 30.0) { 603 | closestHistoryFrameID = id; 604 | break; 605 | } 606 | } 607 | if (closestHistoryFrameID == -1) { 608 | return false; 609 | } 610 | // save latest key frames 611 | latestFrameIDLoopCloure = cloudKeyPoses3D->points.size() - 1; 612 | *latestSurfKeyFrameCloud += 613 | *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure], 614 | &cloudKeyPoses6D->points[latestFrameIDLoopCloure]); 615 | *latestSurfKeyFrameCloud += 616 | *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure], 617 | &cloudKeyPoses6D->points[latestFrameIDLoopCloure]); 618 | 619 | pcl::PointCloud::Ptr hahaCloud(new pcl::PointCloud()); 620 | int cloudSize = latestSurfKeyFrameCloud->points.size(); 621 | for (int i = 0; i < cloudSize; ++i) { 622 | if ((int)latestSurfKeyFrameCloud->points[i].intensity >= 0) { 623 | hahaCloud->push_back(latestSurfKeyFrameCloud->points[i]); 624 | } 625 | } 626 | latestSurfKeyFrameCloud->clear(); 627 | *latestSurfKeyFrameCloud = *hahaCloud; 628 | // save history near key frames 629 | for (int j = - _history_keyframe_search_num; j <= _history_keyframe_search_num; ++j) { 630 | if (closestHistoryFrameID + j < 0 || 631 | closestHistoryFrameID + j > latestFrameIDLoopCloure) 632 | continue; 633 | *nearHistorySurfKeyFrameCloud += *transformPointCloud( 634 | cornerCloudKeyFrames[closestHistoryFrameID + j], 635 | &cloudKeyPoses6D->points[closestHistoryFrameID + j]); 636 | *nearHistorySurfKeyFrameCloud += *transformPointCloud( 637 | surfCloudKeyFrames[closestHistoryFrameID + j], 638 | &cloudKeyPoses6D->points[closestHistoryFrameID + j]); 639 | } 640 | 641 | downSizeFilterHistoryKeyFrames.setInputCloud(nearHistorySurfKeyFrameCloud); 642 | downSizeFilterHistoryKeyFrames.filter(*nearHistorySurfKeyFrameCloudDS); 643 | // publish history near key frames 644 | if (pubHistoryKeyFrames.getNumSubscribers() != 0) { 645 | sensor_msgs::PointCloud2 cloudMsgTemp; 646 | pcl::toROSMsg(*nearHistorySurfKeyFrameCloudDS, cloudMsgTemp); 647 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 648 | cloudMsgTemp.header.frame_id = "/camera_init"; 649 | pubHistoryKeyFrames.publish(cloudMsgTemp); 650 | } 651 | 652 | return true; 653 | } 654 | 655 | void MapOptimization::performLoopClosure() { 656 | 657 | if (cloudKeyPoses3D->points.empty() == true) 658 | return; 659 | 660 | 661 | // try to find close key frame if there are any 662 | if (potentialLoopFlag == false) { 663 | if (detectLoopClosure() == true) { 664 | potentialLoopFlag = true; // find some key frames that is old enough or 665 | // close enough for loop closure 666 | timeSaveFirstCurrentScanForLoopClosure = timeLaserOdometry; 667 | } 668 | if (potentialLoopFlag == false) return; 669 | } 670 | // reset the flag first no matter icp successes or not 671 | potentialLoopFlag = false; 672 | // ICP Settings 673 | pcl::IterativeClosestPoint icp; 674 | icp.setMaxCorrespondenceDistance(100); 675 | icp.setMaximumIterations(100); 676 | icp.setTransformationEpsilon(1e-6); 677 | icp.setEuclideanFitnessEpsilon(1e-6); 678 | icp.setRANSACIterations(0); 679 | // Align clouds 680 | icp.setInputSource(latestSurfKeyFrameCloud); 681 | icp.setInputTarget(nearHistorySurfKeyFrameCloudDS); 682 | pcl::PointCloud::Ptr unused_result( 683 | new pcl::PointCloud()); 684 | icp.align(*unused_result); 685 | 686 | if (icp.hasConverged() == false || 687 | icp.getFitnessScore() > _history_keyframe_fitness_score) 688 | return; 689 | // publish corrected cloud 690 | if (pubIcpKeyFrames.getNumSubscribers() != 0) { 691 | pcl::PointCloud::Ptr closed_cloud( 692 | new pcl::PointCloud()); 693 | pcl::transformPointCloud(*latestSurfKeyFrameCloud, *closed_cloud, 694 | icp.getFinalTransformation()); 695 | sensor_msgs::PointCloud2 cloudMsgTemp; 696 | pcl::toROSMsg(*closed_cloud, cloudMsgTemp); 697 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 698 | cloudMsgTemp.header.frame_id = "/camera_init"; 699 | pubIcpKeyFrames.publish(cloudMsgTemp); 700 | } 701 | /* 702 | get pose constraint 703 | */ 704 | float x, y, z, roll, pitch, yaw; 705 | Eigen::Affine3f correctionCameraFrame; 706 | correctionCameraFrame = 707 | icp.getFinalTransformation(); // get transformation in camera frame 708 | // (because points are in camera frame) 709 | pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, 710 | yaw); 711 | Eigen::Affine3f correctionLidarFrame = 712 | pcl::getTransformation(z, x, y, yaw, roll, pitch); 713 | // transform from world origin to wrong pose 714 | Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar( 715 | cloudKeyPoses6D->points[latestFrameIDLoopCloure]); 716 | // transform from world origin to corrected pose 717 | Eigen::Affine3f tCorrect = 718 | correctionLidarFrame * 719 | tWrong; // pre-multiplying -> successive rotation about a fixed frame 720 | pcl::getTranslationAndEulerAngles(tCorrect, x, y, z, roll, pitch, yaw); 721 | gtsam::Pose3 poseFrom = 722 | Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)); 723 | gtsam::Pose3 poseTo = 724 | pclPointTogtsamPose3(cloudKeyPoses6D->points[closestHistoryFrameID]); 725 | gtsam::Vector Vector6(6); 726 | float noiseScore = icp.getFitnessScore(); 727 | Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, 728 | noiseScore; 729 | auto constraintNoise = noiseModel::Diagonal::Variances(Vector6); 730 | /* 731 | add constraints 732 | */ 733 | std::lock_guard lock(mtx); 734 | gtSAMgraph.add( 735 | BetweenFactor(latestFrameIDLoopCloure, closestHistoryFrameID, 736 | poseFrom.between(poseTo), constraintNoise)); 737 | isam->update(gtSAMgraph); 738 | isam->update(); 739 | gtSAMgraph.resize(0); 740 | 741 | aLoopIsClosed = true; 742 | } 743 | 744 | void MapOptimization::extractSurroundingKeyFrames() { 745 | if (cloudKeyPoses3D->points.empty() == true) return; 746 | 747 | if (_loop_closure_enabled == true) { 748 | // only use recent key poses for graph building 749 | if (recentCornerCloudKeyFrames.size() < 750 | _surrounding_keyframe_search_num) { // queue is not full (the beginning 751 | // of mapping or a loop is just 752 | // closed) 753 | // clear recent key frames queue 754 | recentCornerCloudKeyFrames.clear(); 755 | recentSurfCloudKeyFrames.clear(); 756 | recentOutlierCloudKeyFrames.clear(); 757 | int numPoses = cloudKeyPoses3D->points.size(); 758 | for (int i = numPoses - 1; i >= 0; --i) { 759 | int thisKeyInd = (int)cloudKeyPoses3D->points[i].intensity; 760 | PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd]; 761 | updateTransformPointCloudSinCos(&thisTransformation); 762 | // extract surrounding map 763 | recentCornerCloudKeyFrames.push_front( 764 | transformPointCloud(cornerCloudKeyFrames[thisKeyInd])); 765 | recentSurfCloudKeyFrames.push_front( 766 | transformPointCloud(surfCloudKeyFrames[thisKeyInd])); 767 | recentOutlierCloudKeyFrames.push_front( 768 | transformPointCloud(outlierCloudKeyFrames[thisKeyInd])); 769 | if (recentCornerCloudKeyFrames.size() >= _surrounding_keyframe_search_num) 770 | break; 771 | } 772 | } else { // queue is full, pop the oldest key frame and push the latest 773 | // key frame 774 | if (latestFrameID != cloudKeyPoses3D->points.size()-1) { 775 | // if the robot is not moving, no need to 776 | // update recent frames 777 | 778 | recentCornerCloudKeyFrames.pop_front(); 779 | recentSurfCloudKeyFrames.pop_front(); 780 | recentOutlierCloudKeyFrames.pop_front(); 781 | // push latest scan to the end of queue 782 | latestFrameID = cloudKeyPoses3D->points.size() - 1; 783 | PointTypePose thisTransformation = 784 | cloudKeyPoses6D->points[latestFrameID]; 785 | updateTransformPointCloudSinCos(&thisTransformation); 786 | recentCornerCloudKeyFrames.push_back( 787 | transformPointCloud(cornerCloudKeyFrames[latestFrameID])); 788 | recentSurfCloudKeyFrames.push_back( 789 | transformPointCloud(surfCloudKeyFrames[latestFrameID])); 790 | recentOutlierCloudKeyFrames.push_back( 791 | transformPointCloud(outlierCloudKeyFrames[latestFrameID])); 792 | } 793 | } 794 | 795 | for (int i = 0; i < recentCornerCloudKeyFrames.size(); ++i) { 796 | *laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i]; 797 | *laserCloudSurfFromMap += *recentSurfCloudKeyFrames[i]; 798 | *laserCloudSurfFromMap += *recentOutlierCloudKeyFrames[i]; 799 | } 800 | } else { 801 | surroundingKeyPoses->clear(); 802 | surroundingKeyPosesDS->clear(); 803 | // extract all the nearby key poses and downsample them 804 | kdtreeSurroundingKeyPoses.setInputCloud(cloudKeyPoses3D); 805 | kdtreeSurroundingKeyPoses.radiusSearch( 806 | currentRobotPosPoint, (double)_surrounding_keyframe_search_radius, 807 | pointSearchInd, pointSearchSqDis); 808 | 809 | for (int i = 0; i < pointSearchInd.size(); ++i){ 810 | surroundingKeyPoses->points.push_back( 811 | cloudKeyPoses3D->points[pointSearchInd[i]]); 812 | } 813 | 814 | downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses); 815 | downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS); 816 | 817 | // delete key frames that are not in surrounding region 818 | int numSurroundingPosesDS = surroundingKeyPosesDS->points.size(); 819 | for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) { 820 | bool existingFlag = false; 821 | for (int j = 0; j < numSurroundingPosesDS; ++j) { 822 | if (surroundingExistingKeyPosesID[i] == 823 | (int)surroundingKeyPosesDS->points[j].intensity) { 824 | existingFlag = true; 825 | break; 826 | } 827 | } 828 | if (existingFlag == false) { 829 | surroundingExistingKeyPosesID.erase( 830 | surroundingExistingKeyPosesID.begin() + i); 831 | surroundingCornerCloudKeyFrames.erase( 832 | surroundingCornerCloudKeyFrames.begin() + i); 833 | surroundingSurfCloudKeyFrames.erase( 834 | surroundingSurfCloudKeyFrames.begin() + i); 835 | surroundingOutlierCloudKeyFrames.erase( 836 | surroundingOutlierCloudKeyFrames.begin() + i); 837 | --i; 838 | } 839 | } 840 | // add new key frames that are not in calculated existing key frames 841 | for (int i = 0; i < numSurroundingPosesDS; ++i) { 842 | bool existingFlag = false; 843 | for (auto iter = surroundingExistingKeyPosesID.begin(); 844 | iter != surroundingExistingKeyPosesID.end(); ++iter) { 845 | if ((*iter) == (int)surroundingKeyPosesDS->points[i].intensity) { 846 | existingFlag = true; 847 | break; 848 | } 849 | } 850 | if (existingFlag == true) { 851 | continue; 852 | } else { 853 | int thisKeyInd = (int)surroundingKeyPosesDS->points[i].intensity; 854 | PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd]; 855 | updateTransformPointCloudSinCos(&thisTransformation); 856 | surroundingExistingKeyPosesID.push_back(thisKeyInd); 857 | surroundingCornerCloudKeyFrames.push_back( 858 | transformPointCloud(cornerCloudKeyFrames[thisKeyInd])); 859 | surroundingSurfCloudKeyFrames.push_back( 860 | transformPointCloud(surfCloudKeyFrames[thisKeyInd])); 861 | surroundingOutlierCloudKeyFrames.push_back( 862 | transformPointCloud(outlierCloudKeyFrames[thisKeyInd])); 863 | } 864 | } 865 | 866 | for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) { 867 | *laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i]; 868 | *laserCloudSurfFromMap += *surroundingSurfCloudKeyFrames[i]; 869 | *laserCloudSurfFromMap += *surroundingOutlierCloudKeyFrames[i]; 870 | } 871 | } 872 | // Downsample the surrounding corner key frames (or map) 873 | downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap); 874 | downSizeFilterCorner.filter(*laserCloudCornerFromMapDS); 875 | laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->points.size(); 876 | // Downsample the surrounding surf key frames (or map) 877 | downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap); 878 | downSizeFilterSurf.filter(*laserCloudSurfFromMapDS); 879 | laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->points.size(); 880 | } 881 | 882 | void MapOptimization::downsampleCurrentScan() { 883 | laserCloudCornerLastDS->clear(); 884 | downSizeFilterCorner.setInputCloud(laserCloudCornerLast); 885 | downSizeFilterCorner.filter(*laserCloudCornerLastDS); 886 | laserCloudCornerLastDSNum = laserCloudCornerLastDS->points.size(); 887 | 888 | laserCloudSurfLastDS->clear(); 889 | downSizeFilterSurf.setInputCloud(laserCloudSurfLast); 890 | downSizeFilterSurf.filter(*laserCloudSurfLastDS); 891 | laserCloudSurfLastDSNum = laserCloudSurfLastDS->points.size(); 892 | 893 | laserCloudOutlierLastDS->clear(); 894 | downSizeFilterOutlier.setInputCloud(laserCloudOutlierLast); 895 | downSizeFilterOutlier.filter(*laserCloudOutlierLastDS); 896 | laserCloudOutlierLastDSNum = laserCloudOutlierLastDS->points.size(); 897 | 898 | laserCloudSurfTotalLast->clear(); 899 | laserCloudSurfTotalLastDS->clear(); 900 | *laserCloudSurfTotalLast += *laserCloudSurfLastDS; 901 | *laserCloudSurfTotalLast += *laserCloudOutlierLastDS; 902 | downSizeFilterSurf.setInputCloud(laserCloudSurfTotalLast); 903 | downSizeFilterSurf.filter(*laserCloudSurfTotalLastDS); 904 | laserCloudSurfTotalLastDSNum = laserCloudSurfTotalLastDS->points.size(); 905 | } 906 | 907 | void MapOptimization::cornerOptimization(int iterCount) { 908 | updatePointAssociateToMapSinCos(); 909 | for (int i = 0; i < laserCloudCornerLastDSNum; i++) { 910 | pointOri = laserCloudCornerLastDS->points[i]; 911 | pointAssociateToMap(&pointOri, &pointSel); 912 | kdtreeCornerFromMap.nearestKSearch(pointSel, 5, pointSearchInd, 913 | pointSearchSqDis); 914 | 915 | if (pointSearchSqDis[4] < 1.0) { 916 | float cx = 0, cy = 0, cz = 0; 917 | for (int j = 0; j < 5; j++) { 918 | cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x; 919 | cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y; 920 | cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z; 921 | } 922 | cx /= 5; 923 | cy /= 5; 924 | cz /= 5; 925 | 926 | float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0; 927 | for (int j = 0; j < 5; j++) { 928 | float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx; 929 | float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy; 930 | float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz; 931 | 932 | a11 += ax * ax; 933 | a12 += ax * ay; 934 | a13 += ax * az; 935 | a22 += ay * ay; 936 | a23 += ay * az; 937 | a33 += az * az; 938 | } 939 | a11 /= 5; 940 | a12 /= 5; 941 | a13 /= 5; 942 | a22 /= 5; 943 | a23 /= 5; 944 | a33 /= 5; 945 | 946 | matA1(0, 0) = a11; 947 | matA1(0, 1) = a12; 948 | matA1(0, 2) = a13; 949 | matA1(1, 0) = a12; 950 | matA1(1, 1) = a22; 951 | matA1(1, 2) = a23; 952 | matA1(2, 0) = a13; 953 | matA1(2, 1) = a23; 954 | matA1(2, 2) = a33; 955 | 956 | Eigen::SelfAdjointEigenSolver esolver(matA1); 957 | 958 | matD1 = esolver.eigenvalues().real(); 959 | matV1 = esolver.eigenvectors().real(); 960 | 961 | if (matD1[2] > 3 * matD1[1]) { 962 | float x0 = pointSel.x; 963 | float y0 = pointSel.y; 964 | float z0 = pointSel.z; 965 | float x1 = cx + 0.1 * matV1(0, 0); 966 | float y1 = cy + 0.1 * matV1(0, 1); 967 | float z1 = cz + 0.1 * matV1(0, 2); 968 | float x2 = cx - 0.1 * matV1(0, 0); 969 | float y2 = cy - 0.1 * matV1(0, 1); 970 | float z2 = cz - 0.1 * matV1(0, 2); 971 | 972 | float a012 = sqrt(((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) * 973 | ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) + 974 | ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) * 975 | ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) + 976 | ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1)) * 977 | ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))); 978 | 979 | float l12 = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + 980 | (z1 - z2) * (z1 - z2)); 981 | 982 | float la = 983 | ((y1 - y2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) + 984 | (z1 - z2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1))) / 985 | a012 / l12; 986 | 987 | float lb = 988 | -((x1 - x2) * ((x0 - x1) * (y0 - y2) - (x0 - x2) * (y0 - y1)) - 989 | (z1 - z2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / 990 | a012 / l12; 991 | 992 | float lc = 993 | -((x1 - x2) * ((x0 - x1) * (z0 - z2) - (x0 - x2) * (z0 - z1)) + 994 | (y1 - y2) * ((y0 - y1) * (z0 - z2) - (y0 - y2) * (z0 - z1))) / 995 | a012 / l12; 996 | 997 | float ld2 = a012 / l12; 998 | 999 | float s = 1 - 0.9 * fabs(ld2); 1000 | 1001 | coeff.x = s * la; 1002 | coeff.y = s * lb; 1003 | coeff.z = s * lc; 1004 | coeff.intensity = s * ld2; 1005 | 1006 | if (s > 0.1) { 1007 | laserCloudOri->push_back(pointOri); 1008 | coeffSel->push_back(coeff); 1009 | } 1010 | } 1011 | } 1012 | } 1013 | } 1014 | 1015 | void MapOptimization::surfOptimization(int iterCount) { 1016 | updatePointAssociateToMapSinCos(); 1017 | for (int i = 0; i < laserCloudSurfTotalLastDSNum; i++) { 1018 | pointOri = laserCloudSurfTotalLastDS->points[i]; 1019 | pointAssociateToMap(&pointOri, &pointSel); 1020 | kdtreeSurfFromMap.nearestKSearch(pointSel, 5, pointSearchInd, 1021 | pointSearchSqDis); 1022 | 1023 | if (pointSearchSqDis[4] < 1.0) { 1024 | for (int j = 0; j < 5; j++) { 1025 | matA0(j, 0) = 1026 | laserCloudSurfFromMapDS->points[pointSearchInd[j]].x; 1027 | matA0(j, 1) = 1028 | laserCloudSurfFromMapDS->points[pointSearchInd[j]].y; 1029 | matA0(j, 2) = 1030 | laserCloudSurfFromMapDS->points[pointSearchInd[j]].z; 1031 | } 1032 | matX0 = matA0.colPivHouseholderQr().solve(matB0); 1033 | 1034 | float pa = matX0(0, 0); 1035 | float pb = matX0(1, 0); 1036 | float pc = matX0(2, 0); 1037 | float pd = 1; 1038 | 1039 | float ps = sqrt(pa * pa + pb * pb + pc * pc); 1040 | pa /= ps; 1041 | pb /= ps; 1042 | pc /= ps; 1043 | pd /= ps; 1044 | 1045 | bool planeValid = true; 1046 | for (int j = 0; j < 5; j++) { 1047 | if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x + 1048 | pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y + 1049 | pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + 1050 | pd) > 0.2) { 1051 | planeValid = false; 1052 | break; 1053 | } 1054 | } 1055 | 1056 | if (planeValid) { 1057 | float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; 1058 | 1059 | float s = 1 - 0.9 * fabs(pd2) / 1060 | sqrt(sqrt(pointSel.x * pointSel.x + 1061 | pointSel.y * pointSel.y + 1062 | pointSel.z * pointSel.z)); 1063 | 1064 | coeff.x = s * pa; 1065 | coeff.y = s * pb; 1066 | coeff.z = s * pc; 1067 | coeff.intensity = s * pd2; 1068 | 1069 | if (s > 0.1) { 1070 | laserCloudOri->push_back(pointOri); 1071 | coeffSel->push_back(coeff); 1072 | } 1073 | } 1074 | } 1075 | } 1076 | } 1077 | 1078 | bool MapOptimization::LMOptimization(int iterCount) { 1079 | float srx = sin(transformTobeMapped[0]); 1080 | float crx = cos(transformTobeMapped[0]); 1081 | float sry = sin(transformTobeMapped[1]); 1082 | float cry = cos(transformTobeMapped[1]); 1083 | float srz = sin(transformTobeMapped[2]); 1084 | float crz = cos(transformTobeMapped[2]); 1085 | 1086 | int laserCloudSelNum = laserCloudOri->points.size(); 1087 | if (laserCloudSelNum < 50) { 1088 | return false; 1089 | } 1090 | 1091 | Eigen::Matrix matA(laserCloudSelNum, 6); 1092 | Eigen::Matrix matAt(6,laserCloudSelNum); 1093 | Eigen::Matrix matAtA; 1094 | Eigen::VectorXf matB(laserCloudSelNum); 1095 | Eigen::Matrix matAtB; 1096 | Eigen::Matrix matX; 1097 | 1098 | for (int i = 0; i < laserCloudSelNum; i++) { 1099 | pointOri = laserCloudOri->points[i]; 1100 | coeff = coeffSel->points[i]; 1101 | 1102 | float arx = 1103 | (crx * sry * srz * pointOri.x + crx * crz * sry * pointOri.y - 1104 | srx * sry * pointOri.z) * 1105 | coeff.x + 1106 | (-srx * srz * pointOri.x - crz * srx * pointOri.y - crx * pointOri.z) * 1107 | coeff.y + 1108 | (crx * cry * srz * pointOri.x + crx * cry * crz * pointOri.y - 1109 | cry * srx * pointOri.z) * 1110 | coeff.z; 1111 | 1112 | float ary = 1113 | ((cry * srx * srz - crz * sry) * pointOri.x + 1114 | (sry * srz + cry * crz * srx) * pointOri.y + crx * cry * pointOri.z) * 1115 | coeff.x + 1116 | ((-cry * crz - srx * sry * srz) * pointOri.x + 1117 | (cry * srz - crz * srx * sry) * pointOri.y - crx * sry * pointOri.z) * 1118 | coeff.z; 1119 | 1120 | float arz = ((crz * srx * sry - cry * srz) * pointOri.x + 1121 | (-cry * crz - srx * sry * srz) * pointOri.y) * 1122 | coeff.x + 1123 | (crx * crz * pointOri.x - crx * srz * pointOri.y) * coeff.y + 1124 | ((sry * srz + cry * crz * srx) * pointOri.x + 1125 | (crz * sry - cry * srx * srz) * pointOri.y) * 1126 | coeff.z; 1127 | 1128 | matA(i, 0) = arx; 1129 | matA(i, 1) = ary; 1130 | matA(i, 2) = arz; 1131 | matA(i, 3) = coeff.x; 1132 | matA(i, 4) = coeff.y; 1133 | matA(i, 5) = coeff.z; 1134 | matB(i, 0) = -coeff.intensity; 1135 | } 1136 | matAt = matA.transpose(); 1137 | matAtA = matAt * matA; 1138 | matAtB = matAt * matB; 1139 | matX = matAtA.colPivHouseholderQr().solve(matAtB); 1140 | 1141 | if (iterCount == 0) { 1142 | Eigen::Matrix matE; 1143 | Eigen::Matrix matV; 1144 | Eigen::Matrix matV2; 1145 | 1146 | Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA); 1147 | matE = esolver.eigenvalues().real(); 1148 | matV = esolver.eigenvectors().real(); 1149 | 1150 | matV2 = matV; 1151 | 1152 | isDegenerate = false; 1153 | float eignThre[6] = {100, 100, 100, 100, 100, 100}; 1154 | for (int i = 5; i >= 0; i--) { 1155 | if (matE(0, i) < eignThre[i]) { 1156 | for (int j = 0; j < 6; j++) { 1157 | matV2(i, j) = 0; 1158 | } 1159 | isDegenerate = true; 1160 | } else { 1161 | break; 1162 | } 1163 | } 1164 | matP = matV.inverse() * matV2; 1165 | } 1166 | 1167 | if (isDegenerate) { 1168 | Eigen::Matrix matX2(matX); 1169 | matX2 = matX; 1170 | matX = matP * matX2; 1171 | } 1172 | 1173 | transformTobeMapped[0] += matX(0, 0); 1174 | transformTobeMapped[1] += matX(1, 0); 1175 | transformTobeMapped[2] += matX(2, 0); 1176 | transformTobeMapped[3] += matX(3, 0); 1177 | transformTobeMapped[4] += matX(4, 0); 1178 | transformTobeMapped[5] += matX(5, 0); 1179 | 1180 | float deltaR = sqrt(pow(pcl::rad2deg(matX(0, 0)), 2) + 1181 | pow(pcl::rad2deg(matX(1, 0)), 2) + 1182 | pow(pcl::rad2deg(matX(2, 0)), 2)); 1183 | float deltaT = sqrt(pow(matX(3, 0) * 100, 2) + 1184 | pow(matX(4, 0) * 100, 2) + 1185 | pow(matX(5, 0) * 100, 2)); 1186 | 1187 | if (deltaR < 0.05 && deltaT < 0.05) { 1188 | return true; 1189 | } 1190 | return false; 1191 | } 1192 | 1193 | void MapOptimization::scan2MapOptimization() { 1194 | if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) { 1195 | kdtreeCornerFromMap.setInputCloud(laserCloudCornerFromMapDS); 1196 | kdtreeSurfFromMap.setInputCloud(laserCloudSurfFromMapDS); 1197 | 1198 | for (int iterCount = 0; iterCount < 10; iterCount++) { 1199 | laserCloudOri->clear(); 1200 | coeffSel->clear(); 1201 | 1202 | cornerOptimization(iterCount); 1203 | surfOptimization(iterCount); 1204 | 1205 | if (LMOptimization(iterCount) == true) break; 1206 | } 1207 | 1208 | transformUpdate(); 1209 | } 1210 | } 1211 | 1212 | void MapOptimization::saveKeyFramesAndFactor() { 1213 | currentRobotPosPoint.x = transformAftMapped[3]; 1214 | currentRobotPosPoint.y = transformAftMapped[4]; 1215 | currentRobotPosPoint.z = transformAftMapped[5]; 1216 | 1217 | gtsam::Vector Vector6(6); 1218 | Vector6 << 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-6; 1219 | auto priorNoise = noiseModel::Diagonal::Variances(Vector6); 1220 | auto odometryNoise = noiseModel::Diagonal::Variances(Vector6); 1221 | 1222 | bool saveThisKeyFrame = true; 1223 | if (sqrt((previousRobotPosPoint.x - currentRobotPosPoint.x) * 1224 | (previousRobotPosPoint.x - currentRobotPosPoint.x) + 1225 | (previousRobotPosPoint.y - currentRobotPosPoint.y) * 1226 | (previousRobotPosPoint.y - currentRobotPosPoint.y) + 1227 | (previousRobotPosPoint.z - currentRobotPosPoint.z) * 1228 | (previousRobotPosPoint.z - currentRobotPosPoint.z)) < 0.3) { 1229 | saveThisKeyFrame = false; 1230 | } 1231 | 1232 | if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty()) return; 1233 | 1234 | previousRobotPosPoint = currentRobotPosPoint; 1235 | /** 1236 | * update grsam graph 1237 | */ 1238 | if (cloudKeyPoses3D->points.empty()) { 1239 | gtSAMgraph.add(PriorFactor( 1240 | 0, 1241 | Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], 1242 | transformTobeMapped[1]), 1243 | Point3(transformTobeMapped[5], transformTobeMapped[3], 1244 | transformTobeMapped[4])), 1245 | priorNoise)); 1246 | initialEstimate.insert( 1247 | 0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], 1248 | transformTobeMapped[1]), 1249 | Point3(transformTobeMapped[5], transformTobeMapped[3], 1250 | transformTobeMapped[4]))); 1251 | for (int i = 0; i < 6; ++i) transformLast[i] = transformTobeMapped[i]; 1252 | } else { 1253 | gtsam::Pose3 poseFrom = Pose3( 1254 | Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]), 1255 | Point3(transformLast[5], transformLast[3], transformLast[4])); 1256 | gtsam::Pose3 poseTo = 1257 | Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], 1258 | transformAftMapped[1]), 1259 | Point3(transformAftMapped[5], transformAftMapped[3], 1260 | transformAftMapped[4])); 1261 | gtSAMgraph.add(BetweenFactor( 1262 | cloudKeyPoses3D->points.size() - 1, cloudKeyPoses3D->points.size(), 1263 | poseFrom.between(poseTo), odometryNoise)); 1264 | initialEstimate.insert( 1265 | cloudKeyPoses3D->points.size(), 1266 | Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], 1267 | transformAftMapped[1]), 1268 | Point3(transformAftMapped[5], transformAftMapped[3], 1269 | transformAftMapped[4]))); 1270 | } 1271 | /** 1272 | * update iSAM 1273 | */ 1274 | isam->update(gtSAMgraph, initialEstimate); 1275 | isam->update(); 1276 | 1277 | gtSAMgraph.resize(0); 1278 | initialEstimate.clear(); 1279 | 1280 | /** 1281 | * save key poses 1282 | */ 1283 | PointType thisPose3D; 1284 | PointTypePose thisPose6D; 1285 | Pose3 latestEstimate; 1286 | 1287 | isamCurrentEstimate = isam->calculateEstimate(); 1288 | latestEstimate = 1289 | isamCurrentEstimate.at(isamCurrentEstimate.size() - 1); 1290 | 1291 | thisPose3D.x = latestEstimate.translation().y(); 1292 | thisPose3D.y = latestEstimate.translation().z(); 1293 | thisPose3D.z = latestEstimate.translation().x(); 1294 | thisPose3D.intensity = 1295 | cloudKeyPoses3D->points.size(); // this can be used as index 1296 | cloudKeyPoses3D->push_back(thisPose3D); 1297 | 1298 | thisPose6D.x = thisPose3D.x; 1299 | thisPose6D.y = thisPose3D.y; 1300 | thisPose6D.z = thisPose3D.z; 1301 | thisPose6D.intensity = thisPose3D.intensity; // this can be used as index 1302 | thisPose6D.roll = latestEstimate.rotation().pitch(); 1303 | thisPose6D.pitch = latestEstimate.rotation().yaw(); 1304 | thisPose6D.yaw = latestEstimate.rotation().roll(); // in camera frame 1305 | thisPose6D.time = timeLaserOdometry; 1306 | cloudKeyPoses6D->push_back(thisPose6D); 1307 | /** 1308 | * save updated transform 1309 | */ 1310 | if (cloudKeyPoses3D->points.size() > 1) { 1311 | transformAftMapped[0] = latestEstimate.rotation().pitch(); 1312 | transformAftMapped[1] = latestEstimate.rotation().yaw(); 1313 | transformAftMapped[2] = latestEstimate.rotation().roll(); 1314 | transformAftMapped[3] = latestEstimate.translation().y(); 1315 | transformAftMapped[4] = latestEstimate.translation().z(); 1316 | transformAftMapped[5] = latestEstimate.translation().x(); 1317 | 1318 | for (int i = 0; i < 6; ++i) { 1319 | transformLast[i] = transformAftMapped[i]; 1320 | transformTobeMapped[i] = transformAftMapped[i]; 1321 | } 1322 | } 1323 | 1324 | pcl::PointCloud::Ptr thisCornerKeyFrame( 1325 | new pcl::PointCloud()); 1326 | pcl::PointCloud::Ptr thisSurfKeyFrame( 1327 | new pcl::PointCloud()); 1328 | pcl::PointCloud::Ptr thisOutlierKeyFrame( 1329 | new pcl::PointCloud()); 1330 | 1331 | pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame); 1332 | pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame); 1333 | pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame); 1334 | 1335 | cornerCloudKeyFrames.push_back(thisCornerKeyFrame); 1336 | surfCloudKeyFrames.push_back(thisSurfKeyFrame); 1337 | outlierCloudKeyFrames.push_back(thisOutlierKeyFrame); 1338 | } 1339 | 1340 | void MapOptimization::correctPoses() { 1341 | if (aLoopIsClosed == true) { 1342 | recentCornerCloudKeyFrames.clear(); 1343 | recentSurfCloudKeyFrames.clear(); 1344 | recentOutlierCloudKeyFrames.clear(); 1345 | // update key poses 1346 | int numPoses = isamCurrentEstimate.size(); 1347 | for (int i = 0; i < numPoses; ++i) { 1348 | cloudKeyPoses3D->points[i].x = 1349 | isamCurrentEstimate.at(i).translation().y(); 1350 | cloudKeyPoses3D->points[i].y = 1351 | isamCurrentEstimate.at(i).translation().z(); 1352 | cloudKeyPoses3D->points[i].z = 1353 | isamCurrentEstimate.at(i).translation().x(); 1354 | 1355 | cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x; 1356 | cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y; 1357 | cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z; 1358 | cloudKeyPoses6D->points[i].roll = 1359 | isamCurrentEstimate.at(i).rotation().pitch(); 1360 | cloudKeyPoses6D->points[i].pitch = 1361 | isamCurrentEstimate.at(i).rotation().yaw(); 1362 | cloudKeyPoses6D->points[i].yaw = 1363 | isamCurrentEstimate.at(i).rotation().roll(); 1364 | } 1365 | 1366 | aLoopIsClosed = false; 1367 | } 1368 | } 1369 | 1370 | void MapOptimization::clearCloud() { 1371 | laserCloudCornerFromMap->clear(); 1372 | laserCloudSurfFromMap->clear(); 1373 | laserCloudCornerFromMapDS->clear(); 1374 | laserCloudSurfFromMapDS->clear(); 1375 | } 1376 | 1377 | 1378 | void MapOptimization::run() { 1379 | size_t cycle_count = 0; 1380 | 1381 | while (ros::ok()) { 1382 | AssociationOut association; 1383 | _input_channel.receive(association); 1384 | if( !ros::ok() ) break; 1385 | 1386 | { 1387 | std::lock_guard lock(mtx); 1388 | 1389 | laserCloudCornerLast = association.cloud_corner_last; 1390 | laserCloudSurfLast = association.cloud_surf_last; 1391 | laserCloudOutlierLast = association.cloud_outlier_last; 1392 | 1393 | timeLaserOdometry = association.laser_odometry.header.stamp.toSec(); 1394 | timeLastProcessing = timeLaserOdometry; 1395 | 1396 | OdometryToTransform(association.laser_odometry, transformSum); 1397 | 1398 | transformAssociateToMap(); 1399 | 1400 | extractSurroundingKeyFrames(); 1401 | 1402 | downsampleCurrentScan(); 1403 | 1404 | scan2MapOptimization(); 1405 | 1406 | saveKeyFramesAndFactor(); 1407 | 1408 | correctPoses(); 1409 | 1410 | publishTF(); 1411 | 1412 | publishKeyPosesAndFrames(); 1413 | 1414 | clearCloud(); 1415 | } 1416 | cycle_count++; 1417 | 1418 | if ((cycle_count % 3) == 0) { 1419 | _loop_closure_signal.send(true); 1420 | } 1421 | 1422 | if ((cycle_count % 10) == 0) { 1423 | _publish_global_signal.send(true); 1424 | } 1425 | } 1426 | } 1427 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/transformFusion.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 "transformFusion.h" 34 | 35 | TransformFusion::TransformFusion(ros::NodeHandle& node) : nh(node) { 36 | pubLaserOdometry2 = 37 | nh.advertise("/integrated_to_init", 5); 38 | subLaserOdometry = nh.subscribe( 39 | "/laser_odom_to_init", 5, &TransformFusion::laserOdometryHandler, this); 40 | subOdomAftMapped = nh.subscribe( 41 | "/aft_mapped_to_init", 5, &TransformFusion::odomAftMappedHandler, this); 42 | 43 | laserOdometry2.header.frame_id = "/camera_init"; 44 | laserOdometry2.child_frame_id = "/camera"; 45 | 46 | laserOdometryTrans2.frame_id_ = "/camera_init"; 47 | laserOdometryTrans2.child_frame_id_ = "/camera"; 48 | 49 | map_2_camera_init_Trans.frame_id_ = "/map"; 50 | map_2_camera_init_Trans.child_frame_id_ = "/camera_init"; 51 | 52 | camera_2_base_link_Trans.frame_id_ = "/camera"; 53 | camera_2_base_link_Trans.child_frame_id_ = "/base_link"; 54 | 55 | for (int i = 0; i < 6; ++i) { 56 | transformSum[i] = 0; 57 | transformIncre[i] = 0; 58 | transformMapped[i] = 0; 59 | transformBefMapped[i] = 0; 60 | transformAftMapped[i] = 0; 61 | } 62 | } 63 | 64 | void TransformFusion::transformAssociateToMap() { 65 | float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - 66 | sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 67 | float y1 = transformBefMapped[4] - transformSum[4]; 68 | float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) + 69 | cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]); 70 | 71 | float x2 = x1; 72 | float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1; 73 | float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1; 74 | 75 | transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2; 76 | transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2; 77 | transformIncre[5] = z2; 78 | 79 | float sbcx = sin(transformSum[0]); 80 | float cbcx = cos(transformSum[0]); 81 | float sbcy = sin(transformSum[1]); 82 | float cbcy = cos(transformSum[1]); 83 | float sbcz = sin(transformSum[2]); 84 | float cbcz = cos(transformSum[2]); 85 | 86 | float sblx = sin(transformBefMapped[0]); 87 | float cblx = cos(transformBefMapped[0]); 88 | float sbly = sin(transformBefMapped[1]); 89 | float cbly = cos(transformBefMapped[1]); 90 | float sblz = sin(transformBefMapped[2]); 91 | float cblz = cos(transformBefMapped[2]); 92 | 93 | float salx = sin(transformAftMapped[0]); 94 | float calx = cos(transformAftMapped[0]); 95 | float saly = sin(transformAftMapped[1]); 96 | float caly = cos(transformAftMapped[1]); 97 | float salz = sin(transformAftMapped[2]); 98 | float calz = cos(transformAftMapped[2]); 99 | 100 | float srx = -sbcx * (salx * sblx + calx * cblx * salz * sblz + 101 | calx * calz * cblx * cblz) - 102 | cbcx * sbcy * 103 | (calx * calz * (cbly * sblz - cblz * sblx * sbly) - 104 | calx * salz * (cbly * cblz + sblx * sbly * sblz) + 105 | cblx * salx * sbly) - 106 | cbcx * cbcy * 107 | (calx * salz * (cblz * sbly - cbly * sblx * sblz) - 108 | calx * calz * (sbly * sblz + cbly * cblz * sblx) + 109 | cblx * cbly * salx); 110 | transformMapped[0] = -asin(srx); 111 | 112 | float srycrx = sbcx * (cblx * cblz * (caly * salz - calz * salx * saly) - 113 | cblx * sblz * (caly * calz + salx * saly * salz) + 114 | calx * saly * sblx) - 115 | cbcx * cbcy * 116 | ((caly * calz + salx * saly * salz) * 117 | (cblz * sbly - cbly * sblx * sblz) + 118 | (caly * salz - calz * salx * saly) * 119 | (sbly * sblz + cbly * cblz * sblx) - 120 | calx * cblx * cbly * saly) + 121 | cbcx * sbcy * 122 | ((caly * calz + salx * saly * salz) * 123 | (cbly * cblz + sblx * sbly * sblz) + 124 | (caly * salz - calz * salx * saly) * 125 | (cbly * sblz - cblz * sblx * sbly) + 126 | calx * cblx * saly * sbly); 127 | float crycrx = sbcx * (cblx * sblz * (calz * saly - caly * salx * salz) - 128 | cblx * cblz * (saly * salz + caly * calz * salx) + 129 | calx * caly * sblx) + 130 | cbcx * cbcy * 131 | ((saly * salz + caly * calz * salx) * 132 | (sbly * sblz + cbly * cblz * sblx) + 133 | (calz * saly - caly * salx * salz) * 134 | (cblz * sbly - cbly * sblx * sblz) + 135 | calx * caly * cblx * cbly) - 136 | cbcx * sbcy * 137 | ((saly * salz + caly * calz * salx) * 138 | (cbly * sblz - cblz * sblx * sbly) + 139 | (calz * saly - caly * salx * salz) * 140 | (cbly * cblz + sblx * sbly * sblz) - 141 | calx * caly * cblx * sbly); 142 | transformMapped[1] = 143 | atan2(srycrx / cos(transformMapped[0]), crycrx / cos(transformMapped[0])); 144 | 145 | float srzcrx = 146 | (cbcz * sbcy - cbcy * sbcx * sbcz) * 147 | (calx * salz * (cblz * sbly - cbly * sblx * sblz) - 148 | calx * calz * (sbly * sblz + cbly * cblz * sblx) + 149 | cblx * cbly * salx) - 150 | (cbcy * cbcz + sbcx * sbcy * sbcz) * 151 | (calx * calz * (cbly * sblz - cblz * sblx * sbly) - 152 | calx * salz * (cbly * cblz + sblx * sbly * sblz) + 153 | cblx * salx * sbly) + 154 | cbcx * sbcz * 155 | (salx * sblx + calx * cblx * salz * sblz + calx * calz * cblx * cblz); 156 | float crzcrx = 157 | (cbcy * sbcz - cbcz * sbcx * sbcy) * 158 | (calx * calz * (cbly * sblz - cblz * sblx * sbly) - 159 | calx * salz * (cbly * cblz + sblx * sbly * sblz) + 160 | cblx * salx * sbly) - 161 | (sbcy * sbcz + cbcy * cbcz * sbcx) * 162 | (calx * salz * (cblz * sbly - cbly * sblx * sblz) - 163 | calx * calz * (sbly * sblz + cbly * cblz * sblx) + 164 | cblx * cbly * salx) + 165 | cbcx * cbcz * 166 | (salx * sblx + calx * cblx * salz * sblz + calx * calz * cblx * cblz); 167 | transformMapped[2] = 168 | atan2(srzcrx / cos(transformMapped[0]), crzcrx / cos(transformMapped[0])); 169 | 170 | x1 = cos(transformMapped[2]) * transformIncre[3] - 171 | sin(transformMapped[2]) * transformIncre[4]; 172 | y1 = sin(transformMapped[2]) * transformIncre[3] + 173 | cos(transformMapped[2]) * transformIncre[4]; 174 | z1 = transformIncre[5]; 175 | 176 | x2 = x1; 177 | y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1; 178 | z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1; 179 | 180 | transformMapped[3] = transformAftMapped[3] - (cos(transformMapped[1]) * x2 + 181 | sin(transformMapped[1]) * z2); 182 | transformMapped[4] = transformAftMapped[4] - y2; 183 | transformMapped[5] = transformAftMapped[5] - (-sin(transformMapped[1]) * x2 + 184 | cos(transformMapped[1]) * z2); 185 | } 186 | 187 | void TransformFusion::laserOdometryHandler( 188 | const nav_msgs::Odometry::ConstPtr& laserOdometry) { 189 | currentHeader = laserOdometry->header; 190 | OdometryToTransform(*laserOdometry, transformSum); 191 | 192 | transformAssociateToMap(); 193 | 194 | geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw( 195 | transformMapped[2], -transformMapped[0], -transformMapped[1]); 196 | 197 | laserOdometry2.header.stamp = laserOdometry->header.stamp; 198 | laserOdometry2.pose.pose.orientation.x = -geoQuat.y; 199 | laserOdometry2.pose.pose.orientation.y = -geoQuat.z; 200 | laserOdometry2.pose.pose.orientation.z = geoQuat.x; 201 | laserOdometry2.pose.pose.orientation.w = geoQuat.w; 202 | laserOdometry2.pose.pose.position.x = transformMapped[3]; 203 | laserOdometry2.pose.pose.position.y = transformMapped[4]; 204 | laserOdometry2.pose.pose.position.z = transformMapped[5]; 205 | pubLaserOdometry2.publish(laserOdometry2); 206 | 207 | laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; 208 | laserOdometryTrans2.setRotation( 209 | tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 210 | laserOdometryTrans2.setOrigin( 211 | tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5])); 212 | tfBroadcaster2.sendTransform(laserOdometryTrans2); 213 | } 214 | 215 | void TransformFusion::odomAftMappedHandler( 216 | const nav_msgs::Odometry::ConstPtr& odomAftMapped) { 217 | double roll, pitch, yaw; 218 | geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation; 219 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)) 220 | .getRPY(roll, pitch, yaw); 221 | 222 | transformAftMapped[0] = -pitch; 223 | transformAftMapped[1] = -yaw; 224 | transformAftMapped[2] = roll; 225 | 226 | transformAftMapped[3] = odomAftMapped->pose.pose.position.x; 227 | transformAftMapped[4] = odomAftMapped->pose.pose.position.y; 228 | transformAftMapped[5] = odomAftMapped->pose.pose.position.z; 229 | 230 | transformBefMapped[0] = odomAftMapped->twist.twist.angular.x; 231 | transformBefMapped[1] = odomAftMapped->twist.twist.angular.y; 232 | transformBefMapped[2] = odomAftMapped->twist.twist.angular.z; 233 | 234 | transformBefMapped[3] = odomAftMapped->twist.twist.linear.x; 235 | transformBefMapped[4] = odomAftMapped->twist.twist.linear.y; 236 | transformBefMapped[5] = odomAftMapped->twist.twist.linear.z; 237 | } 238 | -------------------------------------------------------------------------------- /LeGO-LOAM/src/transformFusion.h: -------------------------------------------------------------------------------- 1 | #ifndef TRANSFORMFUSION_H 2 | #define TRANSFORMFUSION_H 3 | 4 | #include "lego_loam/utility.h" 5 | 6 | class TransformFusion { 7 | private: 8 | ros::NodeHandle& nh; 9 | ros::Publisher pubLaserOdometry2; 10 | ros::Subscriber subLaserOdometry; 11 | ros::Subscriber subOdomAftMapped; 12 | 13 | nav_msgs::Odometry laserOdometry2; 14 | tf::StampedTransform laserOdometryTrans2; 15 | tf::TransformBroadcaster tfBroadcaster2; 16 | 17 | tf::StampedTransform map_2_camera_init_Trans; 18 | tf::TransformBroadcaster tfBroadcasterMap2CameraInit; 19 | 20 | tf::StampedTransform camera_2_base_link_Trans; 21 | tf::TransformBroadcaster tfBroadcasterCamera2Baselink; 22 | 23 | float transformSum[6]; 24 | float transformIncre[6]; 25 | float transformMapped[6]; 26 | float transformBefMapped[6]; 27 | float transformAftMapped[6]; 28 | 29 | std_msgs::Header currentHeader; 30 | 31 | public: 32 | TransformFusion(ros::NodeHandle& node); 33 | 34 | void transformAssociateToMap(); 35 | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry); 36 | void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped); 37 | }; 38 | 39 | 40 | 41 | 42 | #endif // TRANSFORMFUSION_H 43 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LeGO-LOAM-BOR 2 | 3 | This is a fork of the original [LeGO-LOAM](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM). 4 | 5 | The original author deserves all the credits, we just use good software engineering practices to 6 | make the code more readable and efficient. 7 | 8 | The purpose of this fork is: 9 | 10 | - To improve the quality of the code, making it more readable, consistent and easier to understand and modify. 11 | - To remove hard-coded values and use proper configuration files to describe the hardware. 12 | - To improve performance, in terms of amount of CPU used to calculate the same result. 13 | - To convert a multi-process application into a single-process / multi-threading one; this makes the algorithm 14 | more deterministic and slightly faster. 15 | - To make it easier and faster to work with rosbags: processing a rosbag should be done at maximum 16 | speed allowed by the CPU and in a deterministic way (usual speed improvement in the order of 5X-10X). 17 | - As a consequence of the previous point, creating unit and regression tests will be easier. 18 | 19 | The purpose of this fork (for the time being) is **not** to modify and/or improve the original algorithm. 20 | 21 | **I am not going to maintain this anymore. I hope people find it useful, but I am not replying to any Issues or question**. 22 | 23 | Fork it and have fun. 24 | 25 | # About the original LeGO-LOAM 26 | This repository contains code for a lightweight and ground optimized lidar odometry and mapping (LeGO-LOAM) system for ROS compatible UGVs. 27 | The system takes in point cloud from a Velodyne VLP-16 Lidar (palced horizontal) and optional IMU data as inputs. 28 | It outputs 6D pose estimation in real-time. A demonstration of the system can be found here -> https://www.youtube.com/watch?v=O3tz_ftHV48 29 | 32 |

33 | drawing 34 |

35 | 36 | ## Dependency 37 | 38 | - [ROS](http://wiki.ros.org/ROS/Installation) (tested with indigo and kinetic) 39 | - [gtsam](https://github.com/borglab/gtsam/releases) (Georgia Tech Smoothing and Mapping library, 4.0.0-alpha2) 40 | ``` 41 | wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.0-alpha2.zip 42 | cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/ 43 | cd ~/Downloads/gtsam-4.0.0-alpha2/ 44 | mkdir build && cd build 45 | cmake .. 46 | sudo make install 47 | ``` 48 | 49 | ## Compile 50 | 51 | You can use the following commands to download and compile the package. 52 | 53 | ``` 54 | cd ~/catkin_ws/src 55 | git clone https://github.com/facontidavide/LeGO-LOAM-BOR.git 56 | cd .. 57 | catkin_make 58 | ``` 59 | 60 | ## The system 61 | 62 | LeGO-LOAM is speficifally optimized for a horizontally placed lidar on a ground vehicle. It assumes there is always a ground plane in the scan. The UGV we are using is Clearpath Jackal. 63 | 64 |

65 | drawing 66 |

67 | 68 | The package performs segmentation before feature extraction. 69 | 70 |

71 | drawing 72 |

73 | 74 | Lidar odometry performs two-step Levenberg Marquardt optimization to get 6D transformation. 75 | 76 |

77 | drawing 78 |

79 | 80 | ## New sensor and configuration 81 | 82 | To customize the behavior of the algorithm or to use a lidar different from VLP-16, edit the file **config/loam_config.yaml**. 83 | 84 | One important thing to keep in mind is that our current implementation for range image projection is only suitable for sensors that have evenly distributed channels. 85 | If you want to use our algorithm with Velodyne VLP-32c or HDL-64e, you need to write your own implementation for such projection. 86 | 87 | If the point cloud is not projected properly, you will lose many points and performance. 88 | 89 | **The IMU has been remove from the original code.** Deal with it. 90 | 91 | ## Run the package 92 | 93 | You may process a rosbag using the following command: 94 | 95 | ``` 96 | roslaunch lego_loam_bor run.launch rosbag:=/path/to/your/rosbag lidar_topic:=/velodyne_points 97 | ``` 98 | 99 | Change the parameters `rosbag`, `lidar_topic` as needed. 100 | 101 | 102 | Some sample bags can be downloaded from [here](https://github.com/RobustFieldAutonomyLab/jackal_dataset_20170608). 103 | 104 | ## New data-set 105 | 106 | This dataset, [Stevens data-set](https://github.com/TixiaoShan/Stevens-VLP16-Dataset), is captured using a Velodyne VLP-16, which is mounted on an UGV - Clearpath Jackal, on Stevens Institute of Technology campus. 107 | The VLP-16 rotation rate is set to 10Hz. This data-set features over 20K scans and many loop-closures. 108 | 109 |

110 | drawing 111 |

112 |

113 | drawing 114 |

115 | 116 | ## Cite *LeGO-LOAM* 117 | 118 | Thank you for citing our *LeGO-LOAM* paper if you use any of this code: 119 | ``` 120 | @inproceedings{legoloam2018, 121 | title={LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain}, 122 | author={Tixiao Shan and Brendan Englot}, 123 | booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 124 | pages={4758-4765}, 125 | year={2018}, 126 | organization={IEEE} 127 | } 128 | ``` 129 | -------------------------------------------------------------------------------- /Shan_Englot_IROS_2018_Preprint.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/facontidavide/LeGO-LOAM-BOR/5fbb287e3817ba315d4012a22561cf2c88164fd5/Shan_Englot_IROS_2018_Preprint.pdf -------------------------------------------------------------------------------- /cloud_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cloud_msgs) 3 | 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | message_generation 7 | geometry_msgs 8 | std_msgs 9 | nav_msgs 10 | ) 11 | 12 | add_message_files( 13 | DIRECTORY msg 14 | FILES 15 | cloud_info.msg 16 | ) 17 | 18 | generate_messages( 19 | DEPENDENCIES 20 | geometry_msgs 21 | std_msgs 22 | nav_msgs 23 | ) 24 | 25 | 26 | catkin_package( 27 | CATKIN_DEPENDS 28 | message_runtime 29 | message_generation 30 | geometry_msgs 31 | std_msgs 32 | nav_msgs 33 | ) 34 | 35 | include_directories( 36 | ${catkin_INCLUDE_DIRS} 37 | ) 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /cloud_msgs/msg/cloud_info.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | int32[] startRingIndex 4 | int32[] endRingIndex 5 | 6 | float32 startOrientation 7 | float32 endOrientation 8 | float32 orientationDiff 9 | 10 | bool[] segmentedCloudGroundFlag # true - ground point, false - other points 11 | uint32[] segmentedCloudColInd # point column index in range image 12 | float32[] segmentedCloudRange # point range 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /cloud_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cloud_msgs 4 | 1.0.0 5 | The cloud_msgs package contains messages. 6 | 7 | Tixiao Shan 8 | 9 | BSD 10 | 11 | catkin 12 | geometry_msgs 13 | std_msgs 14 | message_runtime 15 | message_generation 16 | nav_msgs 17 | 18 | nav_msgs 19 | message_generation 20 | message_runtime 21 | geometry_msgs 22 | std_msgs 23 | 24 | 25 | 26 | 27 | 28 | --------------------------------------------------------------------------------