Go public.

CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.10)
+project(iris_lama VERSION 1.0 LANGUAGES CXX C)
+
+set(CMAKE_CXX_STANDARD 11) # use c++11
+
+# Default to release mode.
+if (NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+
+add_subdirectory(src)
+
+export(TARGETS iris-lama FILE iris_lamaConfig.cmake)
+
+set(CMAKE_EXPORT_PACKAGE_REGISTRY ON)
+export(PACKAGE iris_lama)
+

LICENSE.txt

@@ -0,0 +1,28 @@
+
+Copyright (c) 2019-today, Eurico Farinha Pedrosa, University of Aveiro - Portugal
+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 University of Aveiro 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 OWNER 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

@@ -0,0 +1,75 @@
+LaMa - A Localization and Mapping library.
+==========================================
+https://github.com/iris-ua/iris_lama
+
+Developed and maintained by Eurico Pedrosa, University of Aveiro (C) 2019.
+
+Overview
+--------
+LaMa is a C++11 software library for robotic localization and mapping developed at the **Intelligent Robotics and Systems** (IRIS) Laboratory from the University of Aveiro - Portugal. It includes a framework for 3D volumetric grids (for mapping), a localization algorithm based on scan matching and two SLAM solution (an *Online SLAM* and a *Particle Filter SLAM*).
+
+The main feature is *efficiency*. Low computational effort and low memory usage whenever possible. The minimum viable computer to run our localization and SLAM solutions is a [Raspberry Pi 3 Model B+](https://www.raspberrypi.org/products/raspberry-pi-3-model-b-plus/).
+
+#### Build
+
+To build LaMa, clone it from GitHub and use CMake to build.
+```
+$ git clone https://github.com/iris-ua/iris_lama
+$ cd iris_lama
+$ mkdir build
+$ cd build
+$ cmake ..
+```
+Its only dependency is [Eigen3](http://eigen.tuxfamily.org).
+**Note**: LaMa does not provide any executable. For an example on how to use it, please take a look at our integration with ROS.
+
+#### Integration with ROS 
+
+The source code contains `package.xml` so that it can be used as a library from external ros packages.
+We provide ROS nodes to run the localization and the two SLAM solutions. Please go to [iris_lama_ros](https://github.com/iris-ua/iris_lama_ros) for more information.
+
+
+Sparse-Dense Mapping (SDM)
+--------------------------
+Sparse-Dense Mapping (SDM) is a framework for efficient implementation of 3D volumetric grids. Its divides space into small dense *patches* addressable by a sparse data-structure. To improve memory usage each individual *patch* can be compressed during live operations using lossless data compression (currently [lz4](https://github.com/lz4/lz4) and [Zstandard](https://github.com/facebook/zstd)) with *low overhead*.
+It can be a replacement for [OctoMap](https://octomap.github.io/).
+
+Currently it has the following grid maps implemented:
+* **Distance Map**: It provides the distance to the closest occupied cells in the map. We provide the `DynamicDistanceMap` which is an implementation of the dynamic Euclidean map proposed by:
+    > B. Lau, C. Sprunk, and W. Burgard 
+    > **Efficient Grid-Based Spatial Representations for Robot Navigation in Dynamic Environments**
+    > Robotics and Autonomous Systems, 61 (10), 2013, pp. 1116-1130, Elsevier
+
+* **Occupancy Map**: The most common representation of the environment used in robotics. Three (3) variants of the occupancy map are provided: a `SimpleOccupancyMap` where each cell has a tri-state: *free*, *occupied* or *unknown*: a `ProbabilisticOccupancyMap` that encodes the occupancy probability of each cell with logods; and a `FrequencyOccupancyMap` that tracks the number of times a beam hits or traverses (miss) a cell and calculates a hit/miss ratio.
+
+
+For more information about **SDM** please read
+> Eurico Pedrosa, Artur Pereira, Nuno Lau
+> A Sparse-Dense Approach for Efficient Grid Mapping
+> 2018 IEEE International Conference on Autonomous Robot Systems and Competitions (ICARSC)
+
+Localization based on Scan Matching
+-----------------------------------
+We provide a **fast** scan matching approach to mobile robot localization supported by a continuous likelihood field. It can be used to provide accurate localization for robots equipped with a laser and a *not so good* odometry. Nevertheless, a good odometry is always recommended.
+
+> Eurico Pedrosa, Artur Pereira, Nuno Lau
+> Efficient Localization Based on Scan Matching with a Continuous Likelihood Field
+> 2017 IEEE International Conference on Autonomous Robot Systems and Competitions (ICARSC)
+
+Online SLAM
+-----------
+
+For environments without considerable loops this solution can be accurate and very efficient. It can run in *real time* even on a low-spec computer (we have it running on a turtlebot with a raspberry pi 3B+). It uses our localization algorithm combined with a dynamic likelihood field to incrementally build an occupancy map.
+
+For more information please read
+> Eurico Pedrosa, Artur Pereira, Nuno Lau
+> **A Non-Linear Least Squares Approach to SLAM using a Dynamic Likelihood Field**
+> Journal of Intelligent & Robotic Systems 93 (3-4), 519-532
+
+Multi-threaded Particle Filter SLAM
+--------------------
+
+This Particle Filter SLAM is a RBPF SLAM like [GMapping](https://openslam-org.github.io/) and it is the extension of the Online SLAM solution to multiple particles with multi-thread support. Our solution is capable of parallelizing both the localization and mapping processes. It uses a thread-pool to manage the number of working threads.
+
+Even without multi-threading, our solutions is a lightweight competitor against the heavyweight [GMapping](https://openslam-org.github.io/).
+

include/lama/buffer_compressor.h

@@ -0,0 +1,107 @@
+/*
+ * IRIS Localization and Mapping (LaMa)
+ *
+ * Copyright (c) 2019-today, Eurico Pedrosa, University of Aveiro - Portugal
+ * All rights reserved.
+ * License: New BSD
+ *
+ * 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 University of Aveiro 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 OWNER 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 "types.h"
+
+namespace lama {
+
+/**
+ * Abstract in-memory buffer compressor.
+ */
+struct BufferCompressor {
+
+    virtual ~BufferCompressor(){}
+
+    /**
+     * Compress a memory buffer.
+     *
+     * @param[in]  src       Memory buffer to be compressed.
+     * @param[in]  src_size  The size in bytes of the uncompressed memory buffer.
+     * @param[out] dst       Memory buffer that will hold the compressed data.
+     *                       The buffer must _not_ be allocated, it will be allocated
+     *                       by the function.
+     * @param[in]  buffer    Intermediary buffer used during compression. Make
+     *                       sure that its size is >= compressBound(@p src_size).
+     *                       If buffer is *null* an internal one will be used.
+     *
+     * @return Number of bytes written into @p dst or 0 if the compression fails.
+     */
+    virtual size_t compress(const char* src, size_t src_size, char** dst, char* buffer = 0) = 0;
+
+    /**
+     * Decompress a memory buffer.
+     */
+    virtual size_t decompress(const char* src, size_t src_size, char** dst, size_t dst_size) = 0;
+
+    /**
+     * Calculate the maximum compressed size in worst case scenario.
+     *
+     * @param[in] size
+     *
+     * @return Maximum compressed size.
+     */
+    virtual size_t compressBound(size_t size) = 0;
+
+    /**
+     * Create a clone of the compressor.
+     */
+    virtual BufferCompressor* clone() const = 0;
+};
+
+struct LZ4BufferCompressor : public BufferCompressor {
+
+    virtual ~LZ4BufferCompressor(){}
+
+    size_t compress(const char* src, size_t src_size, char** dst, char* buffer = 0);
+    size_t decompress(const char* src, size_t src_size, char** dst, size_t dst_size);
+    size_t compressBound(size_t size);
+
+    BufferCompressor* clone() const;
+};
+
+struct ZSTDBufferCompressor : public BufferCompressor {
+
+    virtual ~ZSTDBufferCompressor(){}
+
+    size_t compress(const char* src, size_t src_size, char** dst, char* buffer = 0);
+    size_t decompress(const char* src, size_t src_size, char** dst, size_t dst_size);
+
+    size_t compressBound(size_t size);
+
+    BufferCompressor* clone() const;
+};
+
+} /* lama */
+

include/lama/cow_ptr.h

@@ -0,0 +1,122 @@
+/*
+ * IRIS Localization and Mapping (LaMa)
+ *
+ * Copyright (c) 2019-today, Eurico Pedrosa, University of Aveiro - Portugal
+ * All rights reserved.
+ * License: New BSD
+ *
+ * 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 University of Aveiro 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 OWNER 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 <memory>
+#include <mutex>
+
+namespace lama {
+
+// https://en.wikibooks.org/wiki/More_C%2B%2B_Idioms/Copy-on-write
+template <class T>
+class COWPtr {
+public:
+    typedef std::shared_ptr<T> Ptr;
+
+public:
+
+    COWPtr(T* ptr = 0) : refptr_(ptr)
+    {}
+
+    COWPtr(Ptr& refptr) : refptr_(refptr)
+    {}
+
+    COWPtr(const COWPtr<T>& other)
+    {
+        refptr_ = other.refptr_;
+    }
+
+    inline COWPtr<T>& operator=(const COWPtr<T>& other)
+    {
+        refptr_ = other.refptr_;
+        return *this;
+    }
+
+    inline T* get()
+    {
+        return refptr_.get();
+    }
+
+    inline const T* get() const
+    {
+        return refptr_.get();
+    }
+
+    inline bool unique() const
+    {
+        return refptr_.unique();
+    }
+
+    inline long use_count() const
+    {
+        return refptr_.use_count();
+    }
+
+    inline const T* operator->() const
+    {
+        return refptr_.operator->();
+    }
+
+    inline const T* read_only() const
+    {
+        return refptr_.operator->();
+    }
+
+    inline T* operator->()
+    {
+        detach();
+        return refptr_.operator->();
+    }
+
+private:
+
+    void detach()
+    {
+        if (refptr_.unique())
+            return;
+
+        T* tmp = refptr_.get();
+
+        std::unique_lock<std::mutex> lock(mutex_);
+        if (not refptr_.unique())
+            refptr_ = Ptr( new T( *tmp ) );
+    }
+
+private:
+    Ptr refptr_;
+    std::mutex mutex_;
+};
+
+} /* lama */
+