Merge pull request #27 from JMUWRobotics/icpFixpoint_dev

Merge ICP fixpoint after resolving any merge conflicts (there was just one conflict with a single space in code)

Author: yaxy2

3rdparty/CMakeModules/FindSystemC.cmake

@@ -0,0 +1,32 @@
+# FindSystemC.cmake
+
+find_path(SYSTEMC_INCLUDE_DIR
+	NAMES systemc.h
+	PATHS /usr/include
+)
+
+find_library(SYSTEMC_LIBRARY
+	NAMES systemc
+	PATHS /usr/lib /usr/lib/x86_64-linux-gnu
+	
+)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(SystemC DEFAULT_MSG
+	SYSTEMC_LIBRARY	SYSTEMC_INCLUDE_DIR
+)
+
+mark_as_advanced(SYSTEMC_INCLUDE_DIR SYSTEMC_LIBRARY)
+
+if(SystemC_FOUND)
+  set(SystemC_INCLUDE_DIRS ${SYSTEMC_INCLUDE_DIR})
+  set(SystemC_LIBRARIES ${SYSTEMC_LIBRARY})
+
+  if(NOT TARGET SystemC::systemc)
+    add_library(SystemC::systemc UNKNOWN IMPORTED)
+    set_target_properties(SystemC::systemc PROPERTIES
+      IMPORTED_LOCATION "${SYSTEMC_LIBRARY}"
+      INTERFACE_INCLUDE_DIRECTORIES "${SYSTEMC_INCLUDE_DIR}"
+    )
+  endif()
+endif()

CMakeLists.txt

@@ -147,6 +147,7 @@ option(WITH_3DMOUSE "Support for 3DConnexion Space Mouse" OFF)
 option(WITH_TSDF "Complie with TSDF (openVDB)" OFF)
 option(WITH_CALIB "Complie with calibration" OFF)
 option(WITH_SYSTEM_APRILTAG "Link against system-wide apriltag library" OFF)
+option(WITH_SYSTEMC "Compile with SystemC library for fixedPoint arithmetic" ON)
 
 # Adds imgui to show using opengl2 and glut
 if(WITH_IMGUI)
@@ -455,6 +456,7 @@ add_subdirectory(src/gps)
 add_subdirectory(src/curvefusion)
 add_subdirectory(src/mesh)
 add_subdirectory(src/detectCylinder)
+add_subdirectory(src/sc_fixed)
 # 3rdparty must come before src/calibration because it sets
 # APRILTAG_INCLUDE_DIRS
 add_subdirectory(3rdparty)
@@ -474,9 +476,42 @@ find_package(Boost COMPONENTS system filesystem unit_test_framework REQUIRED)
 enable_testing()
 add_subdirectory(testing)
 
+
+# SystemC integration
+
+# Configurable constants for SystemC fixed-point value
+set(FIXED_WORD_LENGTH 48 CACHE STRING "Word length for fixed-point type")
+set(FIXED_INT_WORD_LENGTH 36 CACHE STRING "Integer word length for fixed-point type")
+set(FIXED_HERON_ITERATIONS 3 CACHE STRING "Iterations ins heron algorithm for square root calculations for fixed point numbers")
+
+if(WITH_SYSTEMC)
+  message(STATUS "SystemC support enabled")
+  find_package(SystemC REQUIRED)
+
+  if(SystemC_FOUND)
+    message(STATUS "SystemC installation found")
+    include_directories(${SYSTEMC_INCLUDE_DIR})
+    add_definitions(-DWITH_SYSTEMC)
+    # Set configurable constants for SystemC fixed-point value as compiler definition
+    target_compile_definitions(icpFixpoint PRIVATE
+      FIXED_WORD_LENGTH=${FIXED_WORD_LENGTH}
+      FIXED_INT_WORD_LENGTH=${FIXED_INT_WORD_LENGTH}
+      FIXED_HERON_ITERATIONS=${FIXED_HERON_ITERATIONS}
+    )
+    set(SYSTEMC_LINK_LIBS ${SYSTEMC_LIBRARY})
+
+  else()
+    message(FATAL_ERROR "SystemC was enabled, but could not be found")
+  endif()
+
+  set(SYSTEMC_INCLUDE_DIR)
+
+else()
+  message(STATUS "SystemC support disabled")
+endif()
 # Dummy target with all header files
 # This is a hint for some IDEs, such as Qt Creator, to show all headers in the project tree
 file(GLOB_RECURSE 3DTK_HEADER_FILES "include/*.h")
 add_custom_target(headers SOURCES ${3DTK_HEADER_FILES})
 
-message (STATUS "Build environment is set up!")
+message(STATUS "Build environment is set up!")

doc/icpFixpoint_Entwicklerhandbuch.pdf

@@ -0,0 +1,86 @@
+/** @file
+ *  @brief Representation of 3D scan matching with ICP
+ *  @author Tom Fleischmann, Jonas Wiesner, Yannik Winzer - University of Wuerzburg, Germany
+ */
+
+#ifndef __SC_ICP_H__
+#define __SC_ICP_H__
+
+#include <vector>
+#include <array>
+
+#include "sc_fixed/sc_fixed_math.h"
+#include "sc_fixed/sc_ICPminimizer.h"
+
+/**
+ * Manages the matching of 3D scans.
+ * Important values, such as maximal matching distance,
+ * maximal number of iterations, etc.
+ * are specified in the constructor.
+ */
+class sc_ICP {
+
+public:
+  /**
+   * Constructor
+   */
+  sc_ICP(sc_ICPminimizer *my_sc_ICPminimizer,
+	   f_float max_dist_match = 25.0,
+	   int max_num_iterations = 50,
+	   bool quiet = false,
+	   int epsilonICPexp = 3);
+
+  /**
+   * Destructor (empty, but needed, because virtual)
+   */
+  virtual ~sc_ICP() {};
+
+  virtual int match(std::vector<std::array<f_float, 3>>& source, std::vector<std::array<f_float, 3>>& target, std::array<f_float, 16>& transMat, std::array<f_float, 16>& dalignxf, std::ofstream& frame);
+
+  inline f_float get_max_dist_match2();
+  inline void set_max_dist_match2(f_float max_dist_match2);
+  inline void set_max_num_iterations(int max_num_iterations);
+  inline int get_nr_pointPair();
+
+protected:
+
+  /**
+   * suppress output to cout
+   */
+  bool quiet;
+
+  /**
+   * the maximal distance (^2 !!!) for matching
+   */
+  f_float max_dist_match2;
+
+  /**
+   * the maximal number of iterations
+   */
+  int max_num_iterations;
+
+  /**
+   * epsilon for stopping ICP algorithm ( convergence criterium )
+   */
+  int epsilonICPexp;
+  f_float epsilonICP;
+
+  /**
+   * ptr to ICP error function minimizer functor
+   */
+  sc_ICPminimizer *my_sc_ICPminimizer;
+
+  /**
+   * Maximum number of points in all scans
+   */
+  unsigned int max_scn_size; //FIXME -> update with metascan
+
+  /**
+   * number of matched points in ICP
+   */
+  int nr_pointPair;
+};
+
+#include "sc_fixed/sc_ICP.icc"
+
+#endif //__SC_ICP_H__

doc/icpFixpoint_Pflichtenheft.pdf

@@ -0,0 +1,50 @@
+/*
+ * ICP-fixpoint implementation
+ *
+ * Copyright (C) Tom Fleischmann, Jonas Wiesner, Yannik Winzer
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/**
+ * @file
+ * @brief Implementation of 3D scan matching with ICP
+ * @author Tom Fleischmann, Jonas Wiesner, Yannik Winzer. Institute of Computer Science, University of Wuerzburg, Germany.
+ */
+
+/**
+ * Get the maximal distance for matching
+ *
+ * @return the maximal distance for matching
+ */
+inline f_float sc_ICP::get_max_dist_match2() {
+  return max_dist_match2;
+}
+
+/**
+ * Set the maximal distance for matching
+ *
+ * @param max_dist_match2 the maximal distance (^2 !!!) for matching
+ */
+inline void sc_ICP::set_max_dist_match2(f_float max_dist_match2) {
+  this->max_dist_match2 = max_dist_match2;
+}
+
+/**
+ * Set the Maximum number of iterations
+ *
+ * @param max_num_iterations Maximum number of iterations
+ */
+inline void sc_ICP::set_max_num_iterations(int max_num_iterations) {
+  this->max_num_iterations = max_num_iterations;
+}
+
+/**
+ * Get the nr of point pairs
+ *
+ * @return the nr of point pairs
+ */
+inline int sc_ICP::get_nr_pointPair() {
+  return nr_pointPair;
+}

include/sc_fixed/sc_ICP.h

@@ -0,0 +1,37 @@
+/** @file
+ *  @brief Definition of the ICP error function minimization
+ *  @author Tom Fleischmann, Jonas Wiesner, Yannik Winzer - University of Wuerzburg, Germany
+ */
+
+#ifndef __SC_ICPAPX_H__
+#define __SC_ICPAPX_H__
+
+#include "sc_fixed/sc_fixed_math.h"
+#include "sc_fixed/sc_ICPminimizer.h"
+
+/**
+ * @brief Implementation of the ICP error function minimization via
+ *        approximation using the small angle approximation
+ */
+class sc_ICPapx : public sc_ICPminimizer
+{
+public:
+  /**
+   * Constructor
+   */
+  sc_ICPapx(bool quiet = false) : sc_ICPminimizer(quiet) {};
+  /**
+   * Destructor
+   */
+  virtual ~sc_ICPapx() {};
+
+  f_float Align(const std::vector<std::array<f_float, 3>>& matchedSource,
+             const std::vector<std::array<f_float, 3>>& matchedTarget,
+             f_float *alignxf,
+             const std::array<f_float, 3> centerSource,
+             const std::array<f_float, 3> centerTarget) override;
+
+  inline int getAlgorithmID() { return 6; };
+};
+
+#endif

include/sc_fixed/sc_ICP.icc

@@ -0,0 +1,61 @@
+/*
+ * sc_ICPminimizer implementation
+ *
+ * Copyright (C) Tom Fleischmann, Jonas Wiesner, Yannik Winzer
+ *
+ * Released under the GPL version 3.
+ *
+ */
+
+/**
+ * @file
+ * @brief Implementation of the virtual functor for ICP error function minimization
+ * @author Tom Fleischmann, Jonas Wiesner, Yannik Winzer. Institute of Computer Science, University of Wuerzburg, Germany.
+ */
+
+#ifndef __SC_ICPMINIMIZER__
+#define __SC_ICPMINIMIZER__
+
+#include <vector>
+#include "sc_fixed/sc_fixed_math.h"
+
+#include <iostream>
+#include <stdlib.h>
+#include <array>
+
+class sc_ICPminimizer {
+
+public:
+  /**
+   * Constructor
+   */
+  sc_ICPminimizer(bool quiet = false) { this->quiet = quiet; };
+  /**
+   * Destructor
+   */
+  virtual ~sc_ICPminimizer() {};
+
+  /**
+   * aligning the point pairs
+   */
+  // a detailed discussion of the minimization techniques used for this
+  // function is given in:
+  // Andreas Nuechter, Jan Elseberg, Peter Schneider, and Dietrich Paulus.
+  // Study of Parameterizations for the Rigid Body Transformations of The
+  // Scan Registration Problem, Journal Computer Vision and Image
+  // Understanding (CVIU), Elsevier Science, Volume 114, Issue 8,
+  // pp. 963-980, August 2010.
+
+  virtual f_float Align(const std::vector<std::array<f_float, 3>>& matchedSource,
+                     const std::vector<std::array<f_float, 3>>& matchedTarget,
+                     f_float *alignxf,
+                     const std::array<f_float, 3> centerSource,
+                     const std::array<f_float, 3> centerTarget) = 0;
+
+  virtual int getAlgorithmID() = 0;
+
+protected:
+  bool quiet; ///< determines the verbosity
+};
+
+#endif

include/sc_fixed/sc_ICPapx.h

@@ -0,0 +1,14 @@
+#ifndef SC_FIXED_CONVERTER
+#define SC_FIXED_CONVERTER
+
+#include "slam6d/data_types.h"
+#include <vector>
+#include <array>
+#include "sc_fixed/sc_fixed_math.h"
+
+std::vector<std::array<f_float, 3>> array2fixedArray(const DataXYZ &input);
+std::array<f_float, 16> array2fixedArray16(const double input[16]);
+void printPoints(const std::vector<std::array<f_float, 3>>& points);
+void writeFrame(std::ofstream& frame, std::array<f_float, 16>& matrix, int viewFactor);
+
+#endif //SC_FIXED_CONVERTER

include/sc_fixed/sc_ICPminimizer.h

@@ -0,0 +1,37 @@
+#ifndef SC_FIXED_MATH_H
+#define SC_FIXED_MATH_H
+
+#define SC_INCLUDE_FX
+
+#include <systemc.h>
+#include <array>
+#include <cstring>
+#include <iostream>
+
+#ifndef FIXED_WORD_LENGTH
+#define FIXED_WORD_LENGTH 48
+#endif
+
+#ifndef FIXED_INT_WORD_LENGTH
+#define FIXED_INT_WORD_LENGTH 36
+#endif
+
+#ifndef FIXED_HERON_ITERATIONS
+#define FIXED_HERON_ITERATIONS 3
+#endif
+
+using namespace sc_dt;
+using f_float = sc_fixed<FIXED_WORD_LENGTH, FIXED_INT_WORD_LENGTH>;
+
+f_float sc_fixed_heron_sqrt(f_float s);
+bool sc_choldc(f_float A[3][3], f_float diag[3]);
+void sc_cholsl(f_float A[3][3], f_float diag[3], f_float B[3], f_float x[3]);
+
+void transform(std::vector<std::array<f_float, 3>>& scan, f_float alignxf[16], std::array<f_float, 16>& transMat, std::array<f_float, 16>& dalignxf, std::ofstream& frame, int islum);
+void transformPoints(const f_float alignxf[16], std::vector<std::array<f_float, 3>>& scan);
+void transformMatrix(const f_float alignxf[16], std::array<f_float, 16>& transMat, std::array<f_float, 16>& dalignxf);
+void transform3(const f_float alignxf[16], std::array<f_float, 3>& point);
+void MMult(const f_float M1[16], const std::array<f_float, 16>& M2, std::array<f_float, 16>& Mout);
+f_float sc_abs(f_float);
+
+#endif // SC_FIXED_MATH_H