0.0.0

CHANGELOG.rst

@@ -0,0 +1,27 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package map_utils_matlab
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.0 (2020-06-17)
+------------------
+* Merge branch 'feature/sliding_grid' into 'develop'
+  Feature/sliding grid
+  See merge request wtabib/map_utils_matlab!1
+* Updates log_odds_hit and log_odds_miss values
+* Updates disjunctiveUnion functions to setDifference
+* decrease max clamp so that it entropy doesn't become NaN
+* add getRayPoints function which returns voxels along a ray
+* get unknown point cloud and map entropy
+* disjunctive union and intersection functions
+* remove CXX_STANDARD 17 and correct name of library
+* compile only if MATLAB is found on system
+* get parameters from matlab pointer to map_utils::grid3d::Grid3D
+* add bbx_t matlab bindings and functions to interact with sliding grid3d
+* pass width, height, and depth as arguments to the occupancy grid
+* updated parameters using Kshitij's aaai paper
+* update FindMATLAB.cmake to automatically generalize across matlab versions and
+  operating systems.
+* Nate stores quaternions as <w,x,y,z> and ROS does <x,y,z,w>.
+* remove print in mex file
+* initial commit
+* Contributors: Wennie Tabib

CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(map_utils_matlab)
+
+set(CMAKE_CXX_STANDARD 17)
+
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake")
+
+find_package(MATLAB QUIET)
+
+if (MATLAB_FOUND)
+  find_package(OpenCV REQUIRED)
+  find_package(Eigen3 REQUIRED)
+  find_package(Yaml-cpp REQUIRED)
+  find_package(map_utils REQUIRED)
+  find_package(yaml_utils REQUIRED)
+  find_package(mex_class_wrapper REQUIRED)
+  find_package(mex_conversion_utils REQUIRED)
+
+  include_directories(include
+    ${MATLAB_INCLUDE_DIR}
+    ${map_utils_INCLUDE_DIR}
+    ${yaml_utils_INCLUDE_DIR}
+    ${mex_class_wrapper_INCLUDE_DIR}
+    ${mex_conversion_utils_INCLUDE_DIR}
+    ${EIGEN3_INCLUDE_DIRS}
+  )
+
+  # MATLAB Macro definitions
+  add_definitions(/DMATLAB_MEX_FILE)
+  add_definitions(/DMX_COMPAT_32)
+
+  add_library(grid3d_mex SHARED src/grid3d.cc)
+
+  target_Link_libraries(grid3d_mex
+    ${MATLAB_LIBRARIES}
+    ${Boost_LIBRARIES}
+    yaml-cpp
+  )
+
+  if (UNIX AND APPLE)
+    set(MEXEXT ".mexmaci64")
+  elseif (UNIX)
+    set(MEXEXT ".mexa64")
+  endif()
+
+  set_target_properties(grid3d_mex PROPERTIES SUFFIX ${MEXEXT} PREFIX "")
+
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION "include/${PROJECT_NAME}"
+    FILES_MATCHING PATTERN "*.h"
+  )
+endif()

LICENSE

@@ -0,0 +1,28 @@
+BSD 3-Clause License
+
+Copyright (c) 2023, Wennie Tabib, Kshitij Goel
+
+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.

cmake/FindMATLAB.cmake

@@ -0,0 +1,85 @@
+# - this module looks for Matlab
+# Defines:
+#  MATLAB_INCLUDE_DIR: include path for mex.h
+#  MATLAB_LIBRARIES:   required libraries: libmex, libmx
+#  MATLAB_MEX_LIBRARY: path to libmex
+#  MATLAB_MX_LIBRARY:  path to libmx
+
+# Check if environment variable MATLAB_ROOT is set
+IF( "$ENV{MATLAB_ROOT}" STREQUAL "" )
+  # if it's not set, then we look in the standard locations
+  if (APPLE)
+    file(GLOB matlab_bin_directories "/Applications/MATLAB*/bin")
+    set(mex_program_name "mex")
+  else()
+    file(GLOB matlab_bin_directories "/usr/local/MATLAB/*/bin")
+    set(mex_program_name "mex")
+  endif()
+
+  #Reverse list so the highest version (sorted alphabetically) is preferred
+  list(REVERSE matlab_bin_directories)
+  find_program(MEX_COMMAND ${mex_program_name}
+    PATHS ${matlab_bin_directories} ENV PATH
+    NO_DEFAULT_PATH)
+  mark_as_advanced(FORCE MEX_COMMAND)
+
+  get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
+  get_filename_component(mex_path "${MEX_COMMAND}" PATH)
+  get_filename_component(MATLAB_ROOT2 "${mex_path}/.." ABSOLUTE)
+  set(ENV{MATLAB_ROOT} "${MATLAB_ROOT2}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
+endif()
+
+SET(MATLAB_FOUND 0)
+if ( "$ENV{MATLAB_ROOT}" STREQUAL "" )
+  MESSAGE(STATUS "MATLAB_ROOT environment variable not set." )
+  MESSAGE(STATUS "In Linux this can be done in your user .bashrc file by appending the corresponding line, e.g:" )
+  MESSAGE(STATUS "export MATLAB_ROOT=/usr/local/MATLAB/R2012b" )
+  MESSAGE(STATUS "In Windows this can be done by adding system variable, e.g:" )
+  MESSAGE(STATUS "MATLAB_ROOT=D:\\Program Files\\MATLAB\\R2011a" )
+else()
+  set(MATLAB_INCLUDE_DIR $ENV{MATLAB_ROOT}/extern/include)
+  message("${MATLAB_INCLUDE_DIR}")
+  INCLUDE_DIRECTORIES(${MATLAB_INCLUDE_DIR})
+
+  FIND_LIBRARY( MATLAB_MEX_LIBRARY
+    NAMES libmex mex
+    PATHS $ENV{MATLAB_ROOT}/bin
+    PATH_SUFFIXES maci64 glnxa64 glnx86 win64/microsoft win32/microsoft)
+
+  FIND_LIBRARY( MATLAB_MX_LIBRARY
+    NAMES libmx mx
+    PATHS $ENV{MATLAB_ROOT}/bin
+    PATH_SUFFIXES maci64 glnxa64 glnx86 win64/microsoft win32/microsoft)
+
+  if (APPLE)
+    set(mxLibPath "$ENV{MATLAB_ROOT}/bin/maci64")
+    set(MATLAB_MX_LIBRARY "${mxLibPath}/libmx.dylib")
+  endif()
+
+  FIND_LIBRARY( MATLAB_MAT_LIBRARY
+    NAMES libmat mat
+    PATHS $ENV{MATLAB_ROOT}/bin
+    PATH_SUFFIXES maci64 glnxa64 glnx86 win64/microsoft win32/microsoft)
+
+ENDIF()
+
+# This is common to UNIX and Win32:
+SET(MATLAB_LIBRARIES
+  ${MATLAB_MEX_LIBRARY}
+  ${MATLAB_MX_LIBRARY}
+  ${MATLAB_MAT_LIBRARY}
+  )
+
+IF(MATLAB_INCLUDE_DIR AND MATLAB_LIBRARIES)
+  SET(MATLAB_FOUND 1)
+  MESSAGE(STATUS "Matlab libraries will be used")
+ENDIF(MATLAB_INCLUDE_DIR AND MATLAB_LIBRARIES)
+
+MARK_AS_ADVANCED(
+  MATLAB_LIBRARIES
+  MATLAB_MEX_LIBRARY
+  MATLAB_MX_LIBRARY
+  MATLAB_INCLUDE_DIR
+  MATLAB_FOUND
+  MATLAB_ROOT
+  )

colcon.pkg

@@ -0,0 +1,10 @@
+{
+  "name": "map_utils_matlab",
+  "dependencies":
+  {
+    "map_utils",
+    "yaml_utils",
+    "mex_class_wrapper",
+    "mex_conversion_utils"
+  }
+}

config/grid3d.yaml

@@ -0,0 +1,18 @@
+map:
+  resolution: 0.2
+  free_threshold: 0.13
+  occupancy_threshold: 0.7
+  clamping_min: 0.01
+  clamping_max: 0.99
+  probability_hit: 0.7
+  probability_miss: 0.3
+  initial_count: 10
+  block_size: 30
+  lock_at_max_clamp: false
+  width: 600
+  height: 550
+  depth: 150
+  origin_x: -105
+  origin_y: -95
+  origin_z: -10
+  track_changes: false

matlab/Grid3D.m

@@ -0,0 +1,110 @@
+classdef Grid3D < handle
+    properties
+        origin;
+        resolution;
+        width;
+        height;
+        depth;
+
+        log_odds_hit; log_odds_miss;
+        min_clamp; max_clamp;
+        track_changes;
+        occupancy_threshold;
+        free_threshold;
+        block_size;
+	lock_at_max_clamp;
+    end
+    properties (SetAccess = private, Hidden = true)
+        objectHandle;
+    end
+    methods (Access = private)
+        function setObjectHandle(this, obj_handle)
+            this.objectHandle = obj_handle;
+        end
+    end
+
+    methods
+        function this = Grid3D(in)
+        % nargin can also be zero
+	  if (isa(in, 'char') || isa(in, 'string'))
+	    this.objectHandle = grid3d_mex('new', in);
+	    this.getParameters();
+	  else
+            this.objectHandle = in;
+	  end
+        end
+        function printParameters(this)
+            grid3d_mex('print_parameters', this.objectHandle);
+        end
+        function [xyz, probs] = getXYZProbability(this)
+            [xyz, probs] = grid3d_mex('get_xyz_probability', this.objectHandle);
+        end
+        function p = probability(this, pt)
+            p = grid3d_mex('probability', this.objectHandle, pt);
+        end
+        function p = logodds(this, idx)
+            p = grid3d_mex('logodds', this.objectHandle, idx);
+        end
+        function idxs = getIndex(this, pts)
+            in = pts;
+            if (size(in,1) ~= 3)
+                in = transpose(pts);
+            end
+            if (size(in,1) ~= 3)
+                error('size of pts should be 3xN');
+            end
+
+            idxs = grid3d_mex('get_index', this.objectHandle(), in);
+        end
+        function [] = getParameters(this)
+            [this.origin, this.resolution, this.width, this.height, this.depth,...
+            this.log_odds_hit, this.log_odds_miss, this.min_clamp, this.max_clamp, ...
+            this.track_changes, this.occupancy_threshold, this.free_threshold, ...
+            this.block_size, this.lock_at_max_clamp] = grid3d_mex('get_parameters', this.objectHandle());
+        end
+        function probabilities = getProbability(this)
+            probabilities = grid3d_mex('get_probability', this.getObjectHandle());
+        end
+        function [objectHandle] = getObjectHandle(this)
+            objectHandle = this.objectHandle;
+        end
+        function [pts] = getPoint(this, idxs)
+            pts = grid3d_mex('get_point', this.objectHandle(), idxs);
+        end
+        function [pts, ret] = getRayPoints(this, st, en)
+            [pts, ret] = grid3d_mex('get_ray_points', this.objectHandle(), st, en);
+        end
+	function ret = inQCell(this, c)
+	  ret = grid3d_mex('in_q_cell', this.objectHandle(), c);
+	end
+	function ret = inQPoint(this, pt)
+	  ret = grid3d_mex('in_q_point', this.objectHandle(), pt);
+	end
+	function out = w2c(this, pt)
+	  out = grid3d_mex('w2c', this.objectHandle(), pt);
+	end
+	function out = getNumChanges(this)
+	  out = grid3d_mex('get_num_changes', this.objectHandle());
+	end
+	function [] = resetChangeSet(this)
+	  grid3d_mex('reset_change_set', this.objectHandle());
+	end
+	function [] = addRay(this, st, en, max_range)
+	  grid3d_mex('add_ray', this.objectHandle(), st, en, max_range);
+	end
+	function [] = plot(this, color, alpha)
+	  if isempty(this.origin)
+	    this.getParameters();
+	  end
+	  bbx = BoundingBox();
+	  bbx.min = this.origin;
+	  bbx.max = this.origin + this.resolution*[this.width; this.height; this.depth];
+	  bbx.plot(color, alpha);
+	end
+        function delete(this)
+            if (this.objectHandle ~= 0)
+                grid3d_mex('delete', this.objectHandle);
+            end
+        end
+    end
+end