Announcement: Thank everyone who has been testing and supporting our …

…LeGO-LOAM package. I have completed my Ph.D. program at Stevens and started a new job at MIT. Due to the limitation of my working bandwidth, the update and answering for the repository may be delayed. Sorry for any inconvenience.

.github/stale.yml

@@ -0,0 +1,17 @@
+# Number of days of inactivity before an issue becomes stale
+daysUntilStale: 60
+# Number of days of inactivity before a stale issue is closed
+daysUntilClose: 7
+# Issues with these labels will never be considered stale
+exemptLabels:
+  - pinned
+  - security
+# Label to use when marking an issue as stale
+staleLabel: wontfix
+# Comment to post when marking an issue as stale. Set to `false` to disable
+markComment: >
+  This issue has been automatically marked as stale because it has not had
+  recent activity. It will be closed if no further activity occurs. Thank you
+  for your contributions.
+# Comment to post when closing a stale issue. Set to `false` to disable
+closeComment: false

LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2018, Robust Field Autonomy Lab
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

LeGO-LOAM/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(lego_loam)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3")
+
+find_package(catkin REQUIRED COMPONENTS
+  tf
+  roscpp
+  rospy
+  cv_bridge
+  image_transport
+
+  pcl_ros
+  pcl_conversions
+
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  nav_msgs
+  cloud_msgs
+)
+
+find_package(GTSAM REQUIRED QUIET)
+find_package(PCL REQUIRED QUIET)
+find_package(OpenCV REQUIRED QUIET)
+
+catkin_package(
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS cloud_msgs
+  DEPENDS PCL
+)
+
+include_directories(
+	include
+	${catkin_INCLUDE_DIRS}
+	${PCL_INCLUDE_DIRS}
+	${OpenCV_INCLUDE_DIRS}
+	${GTSAM_INCLUDE_DIR}
+)
+
+link_directories(
+	include
+	${OpenCV_LIBRARY_DIRS}
+	${PCL_LIBRARY_DIRS}
+	${GTSAM_LIBRARY_DIRS}
+)
+
+add_executable(imageProjection src/imageProjection.cpp)
+add_dependencies(imageProjection ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp)
+target_link_libraries(imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
+
+add_executable(featureAssociation src/featureAssociation.cpp)
+add_dependencies(featureAssociation ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp)
+target_link_libraries(featureAssociation ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
+
+add_executable(mapOptmization src/mapOptmization.cpp)
+target_link_libraries(mapOptmization ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam)
+
+add_executable(transformFusion src/transformFusion.cpp)
+target_link_libraries(transformFusion ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})

LeGO-LOAM/include/utility.h

@@ -0,0 +1,161 @@
+#ifndef _UTILITY_LIDAR_ODOMETRY_H_
+#define _UTILITY_LIDAR_ODOMETRY_H_
+
+
+#include <ros/ros.h>
+
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <nav_msgs/Odometry.h>
+
+#include "cloud_msgs/cloud_info.h"
+
+#include <opencv/cv.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/range_image/range_image.h>
+#include <pcl/filters/filter.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/common/common.h>
+#include <pcl/registration/icp.h>
+
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+
+#include <vector>
+#include <cmath>
+#include <algorithm>
+#include <queue>
+#include <deque>
+#include <iostream>
+#include <fstream>
+#include <ctime>
+#include <cfloat>
+#include <iterator>
+#include <sstream>
+#include <string>
+#include <limits>
+#include <iomanip>
+#include <array>
+#include <thread>
+#include <mutex>
+
+#define PI 3.14159265
+
+using namespace std;
+
+typedef pcl::PointXYZI  PointType;
+
+extern const string pointCloudTopic = "/velodyne_points";
+extern const string imuTopic = "/imu/data";
+
+// Save pcd
+extern const string fileDirectory = "/tmp/";
+
+// VLP-16
+extern const int N_SCAN = 16;
+extern const int Horizon_SCAN = 1800;
+extern const float ang_res_x = 0.2;
+extern const float ang_res_y = 2.0;
+extern const float ang_bottom = 15.0+0.1;
+extern const int groundScanInd = 7;
+
+// HDL-32E
+// extern const int N_SCAN = 32;
+// extern const int Horizon_SCAN = 1800;
+// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
+// extern const float ang_res_y = 41.33/float(N_SCAN-1);
+// extern const float ang_bottom = 30.67;
+// extern const int groundScanInd = 20;
+
+// Ouster users may need to uncomment line 159 in imageProjection.cpp
+// Usage of Ouster imu data is not fully supported yet, please just publish point cloud data
+// Ouster OS1-16
+// extern const int N_SCAN = 16;
+// extern const int Horizon_SCAN = 1024;
+// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
+// extern const float ang_res_y = 33.2/float(N_SCAN-1);
+// extern const float ang_bottom = 16.6+0.1;
+// extern const int groundScanInd = 7;
+
+// Ouster OS1-64
+// extern const int N_SCAN = 64;
+// extern const int Horizon_SCAN = 1024;
+// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
+// extern const float ang_res_y = 33.2/float(N_SCAN-1);
+// extern const float ang_bottom = 16.6+0.1;
+// extern const int groundScanInd = 15;
+
+extern const bool loopClosureEnableFlag = false;
+extern const double mappingProcessInterval = 0.3;
+
+extern const float scanPeriod = 0.1;
+extern const int systemDelay = 0;
+extern const int imuQueLength = 200;
+
+extern const float sensorMountAngle = 0.0;
+extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy
+extern const int segmentValidPointNum = 5;
+extern const int segmentValidLineNum = 3;
+extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI;
+extern const float segmentAlphaY = ang_res_y / 180.0 * M_PI;
+
+
+extern const int edgeFeatureNum = 2;
+extern const int surfFeatureNum = 4;
+extern const int sectionsTotal = 6;
+extern const float edgeThreshold = 0.1;
+extern const float surfThreshold = 0.1;
+extern const float nearestFeatureSearchSqDist = 25;
+
+
+// Mapping Params
+extern const float surroundingKeyframeSearchRadius = 50.0; // key frame that is within n meters from current pose will be considerd for scan-to-map optimization (when loop closure disabled)
+extern const int   surroundingKeyframeSearchNum = 50; // submap size (when loop closure enabled)
+// history key frames (history submap for loop closure)
+extern const float historyKeyframeSearchRadius = 7.0; // key frame that is within n meters from current pose will be considerd for loop closure
+extern const int   historyKeyframeSearchNum = 25; // 2n+1 number of hostory key frames will be fused into a submap for loop closure
+extern const float historyKeyframeFitnessScore = 0.3; // the smaller the better alignment
+
+extern const float globalMapVisualizationSearchRadius = 500.0; // key frames with in n meters will be visualized
+
+
+struct smoothness_t{ 
+    float value;
+    size_t ind;
+};
+
+struct by_value{ 
+    bool operator()(smoothness_t const &left, smoothness_t const &right) { 
+        return left.value < right.value;
+    }
+};
+
+/*
+    * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
+    */
+struct PointXYZIRPYT
+{
+    PCL_ADD_POINT4D
+    PCL_ADD_INTENSITY;
+    float roll;
+    float pitch;
+    float yaw;
+    double time;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
+                                   (float, x, x) (float, y, y)
+                                   (float, z, z) (float, intensity, intensity)
+                                   (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
+                                   (double, time, time)
+)
+
+typedef PointXYZIRPYT  PointTypePose;
+
+#endif

LeGO-LOAM/launch/run.launch

@@ -0,0 +1,19 @@
+<launch>
+
+    <!--- Sim Time -->
+    <param name="/use_sim_time" value="true" />
+
+    <!--- Run Rviz-->
+    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lego_loam)/launch/test.rviz" />
+
+    <!--- TF -->
+    <node pkg="tf" type="static_transform_publisher" name="camera_init_to_map"  args="0 0 0 1.570795   0        1.570795 /map    /camera_init 10" />
+    <node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" args="0 0 0 -1.570795 -1.570795 0        /camera /base_link   10" />
+
+    <!--- LeGO-LOAM -->    
+    <node pkg="lego_loam" type="imageProjection"    name="imageProjection"    output="screen"/>
+    <node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/>
+    <node pkg="lego_loam" type="mapOptmization"     name="mapOptmization"     output="screen"/>
+    <node pkg="lego_loam" type="transformFusion"    name="transformFusion"    output="screen"/>
+
+</launch>