first_commit

CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(light_loam)
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-std=c++14")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+# set(CMAKE_CXX_STANDARD 14)
+# set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  roscpp
+  rospy
+  rosbag
+  std_msgs
+  image_transport
+  cv_bridge
+  tf
+)
+
+#find_package(Eigen3 REQUIRED)
+find_package(PCL REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Ceres REQUIRED)
+FIND_PACKAGE(OpenMP REQUIRED)
+if(OPENMP_FOUND)
+    message("OPENMP FOUND")
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+include_directories(
+  include
+	${catkin_INCLUDE_DIRS} 
+	${PCL_INCLUDE_DIRS}
+  ${CERES_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${OpenMP_INCLUDE_DIR})
+
+catkin_package(
+  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
+  DEPENDS EIGEN3 PCL 
+  INCLUDE_DIRS include
+)
+
+
+add_executable(ascanRegistration src/scanRegistration.cpp)
+target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+
+add_executable(alaserOdometry src/laserOdometry.cpp)
+target_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${OpenMP_LIBRARY_DIRS})
+
+add_executable(alaserMapping src/laserMapping.cpp)
+target_link_libraries(alaserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})
+
+add_executable(kittiHelper src/kittiHelper.cpp)
+target_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})
+
+
+
+

LICENSE

@@ -0,0 +1,35 @@
+This is an advanced implementation of the algorithm described in the following paper:
+  J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
+    Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 
+
+Modifier: Tong Qin               [email protected]
+	  Shaozu Cao 		 [email protected]
+
+Copyright 2013, Ji Zhang, Carnegie Mellon University
+Further contributions copyright (c) 2016, Southwest Research Institute
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+2. 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.
+3. 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.
+
+

README.md

@@ -1 +1,14 @@
-# Light-LOAM
+# Light-LOAM
+This is the implementation for the Paper ``Light-LOAM: A Lightweight LiDAR Odometry and Mapping based on Graph-Matching''. This code is modified from [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM).
+
+## Requirements
+* PCL
+* ROS
+* ceres
+
+## Introduction
+This is the beta version, and the final implementation code would be published later. The research paper [CoT: Cooperative Training for Generative Modeling of Discrete Data](https://arxiv.org/abs/1804.03782) is now available on arXiv. Watch our demo at [Video Link](https://youtu.be/PzZly1SQtng). 
+
+
+## Acknowledgements
+Thanks for [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM).

docker/Dockerfile

@@ -0,0 +1,60 @@
+FROM ros:kinetic-perception
+
+ENV CERES_VERSION="1.12.0"
+ENV PCL_VERSION="1.8.0"
+ENV CATKIN_WS=/root/catkin_ws
+
+    # setup processors number used to compile library
+RUN if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; else export USE_PROC=$(($(nproc)/2)) ; fi && \
+    # Install dependencies
+      apt-get update && apt-get install -y \
+      cmake \
+      libatlas-base-dev \
+      libeigen3-dev \
+      libgoogle-glog-dev \
+      libsuitesparse-dev \
+      python-catkin-tools \
+      ros-${ROS_DISTRO}-cv-bridge \
+      ros-${ROS_DISTRO}-image-transport \
+      ros-${ROS_DISTRO}-message-filters \
+      ros-${ROS_DISTRO}-tf && \
+    rm -rf /var/lib/apt/lists/* && \
+    # Build and install Ceres
+    git clone https://ceres-solver.googlesource.com/ceres-solver && \
+    cd ceres-solver && \
+    git checkout tags/${CERES_VERSION} && \
+    mkdir build && cd build && \
+    cmake .. && \
+    make -j${USE_PROC} install && \
+    cd ../.. && \
+    rm -rf ./ceres-solver && \
+    # Build and install pcl
+    git clone https://github.com/PointCloudLibrary/pcl.git && \
+    cd pcl && \
+    git checkout tags/pcl-${PCL_VERSION} && \
+    mkdir build && cd build && \
+    cmake .. && \
+    make -j${USE_PROC} install && \
+    cd ../.. && \
+    rm -rf ./pcl && \
+    # Setup catkin workspace
+    mkdir -p $CATKIN_WS/src/A-LOAM/
+
+# WORKDIR $CATKIN_WS/src
+
+# Copy A-LOAM
+COPY ./ $CATKIN_WS/src/A-LOAM/
+# use the following line if you only have this dockerfile
+# RUN git clone https://github.com/HKUST-Aerial-Robotics/A-LOAM.git
+
+# Build A-LOAM
+WORKDIR $CATKIN_WS
+ENV TERM xterm
+ENV PYTHONIOENCODING UTF-8
+RUN catkin config \
+      --extend /opt/ros/$ROS_DISTRO \
+      --cmake-args \
+        -DCMAKE_BUILD_TYPE=Release && \
+    catkin build && \
+    sed -i '/exec "$@"/i \
+            source "/root/catkin_ws/devel/setup.bash"' /ros_entrypoint.sh

docker/Makefile

@@ -0,0 +1,16 @@
+all: help
+
+help:
+	@echo ""
+	@echo "-- Help Menu"
+	@echo ""
+	@echo "   1. make build              - build all images"
+	# @echo "   1. make pull             - pull all images"
+	@echo "   1. make clean              - remove all images"
+	@echo ""
+
+build:
+	@docker build --tag ros:aloam -f ./Dockerfile ..
+
+clean:
+	@docker rmi -f ros:aloam

docker/run.sh

@@ -0,0 +1,91 @@
+#!/bin/bash
+trap : SIGTERM SIGINT
+
+function abspath() {
+    # generate absolute path from relative path
+    # $1     : relative filename
+    # return : absolute path
+    if [ -d "$1" ]; then
+        # dir
+        (cd "$1"; pwd)
+    elif [ -f "$1" ]; then
+        # file
+        if [[ $1 = /* ]]; then
+            echo "$1"
+        elif [[ $1 == */* ]]; then
+            echo "$(cd "${1%/*}"; pwd)/${1##*/}"
+        else
+            echo "$(pwd)/$1"
+        fi
+    fi
+}
+
+if [ "$#" -ne 1 ]; then
+  echo "Usage: $0 LIDAR_SCAN_NUMBER" >&2
+  exit 1
+fi
+
+roscore &
+ROSCORE_PID=$!
+sleep 1
+
+rviz -d ../rviz_cfg/aloam_velodyne.rviz &
+RVIZ_PID=$!
+
+A_LOAM_DIR=$(abspath "..")
+
+if [ "$1" -eq 16 ]; then
+    docker run \
+    -it \
+    --rm \
+    --net=host \
+    -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \
+    ros:aloam \
+    /bin/bash -c \
+    "cd /root/catkin_ws/; \
+    catkin config \
+        --cmake-args \
+            -DCMAKE_BUILD_TYPE=Release; \
+        catkin build; \
+        source devel/setup.bash; \
+        roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch rviz:=false"
+elif [ "$1" -eq "32" ]; then
+    docker run \
+    -it \
+    --rm \
+    --net=host \
+    -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \
+    ros:aloam \
+    /bin/bash -c \
+    "cd /root/catkin_ws/; \
+    catkin config \
+        --cmake-args \
+            -DCMAKE_BUILD_TYPE=Release; \
+        catkin build; \
+        source devel/setup.bash; \
+        roslaunch aloam_velodyne aloam_velodyne_HDL_32.launch rviz:=false"
+elif [ "$1" -eq "64" ]; then
+    docker run \
+    -it \
+    --rm \
+    --net=host \
+    -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \
+    ros:aloam \
+    /bin/bash -c \
+    "cd /root/catkin_ws/; \
+    catkin config \
+        --cmake-args \
+            -DCMAKE_BUILD_TYPE=Release; \
+        catkin build; \
+        source devel/setup.bash; \
+        roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch rviz:=false"
+fi
+
+wait $ROSCORE_PID
+wait $RVIZ_PID
+
+if [[ $? -gt 128 ]]
+then
+    kill $ROSCORE_PID
+    kill $RVIZ_PID
+fi

include/aloam_velodyne/common.h

@@ -0,0 +1,89 @@
+// This is an advanced implementation of the algorithm described in the following paper:
+//   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
+//     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 
+
+// Modifier: Tong Qin               [email protected]
+// 	         Shaozu Cao 		    [email protected]
+
+
+// Copyright 2013, Ji Zhang, Carnegie Mellon University
+// Further contributions copyright (c) 2016, Southwest Research Institute
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// 1. Redistributions of source code must retain the above copyright notice,
+//    this list of conditions and the following disclaimer.
+// 2. 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.
+// 3. 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.
+
+#pragma once
+
+#include <cmath>
+
+#include <pcl/point_types.h>
+
+typedef pcl::PointXYZI PointType;
+
+inline double rad2deg(double radians)
+{
+  return radians * 180.0 / M_PI;
+}
+
+inline double deg2rad(double degrees)
+{
+  return degrees * M_PI / 180.0;
+}
+
+//new add
+typedef struct {
+    int index;
+    // int tgt_idx;
+    pcl::PointXYZI src;
+    pcl::PointXYZI tgt;
+    // Eigen::Vector3f src_norm;
+    // Eigen::Vector3f tgt_norm;
+    // Eigen::Matrix3f covariance_src;
+    // Eigen::Matrix3f covariance_tgt;
+    float score;
+    float s; // optimization weight
+}Corre_Match;
+
+typedef struct 
+{
+    int vertex_index;
+    std::vector<int> connected_vertex_index;
+    int degree;
+}Vertex_Attribute;
+
+typedef struct{
+    int index;
+    float score;
+}Vertex_Vote;
+
+// bool compare_degree(const Vertex_Attribute ob1, const Vertex_Attribute ob2)
+// {
+//     return ob1.degree > ob2.degree;
+// }
+
+struct compare_score{
+  bool operator()(Vertex_Vote const &ob1, Vertex_Vote const &ob2){return ob1.score > ob2.score;}
+};
+//new add