diff --git a/CMakeLists.txt b/CMakeLists.txt
deleted file mode 100644
index f32ffb5..0000000
--- a/CMakeLists.txt
+++ /dev/null
@@ -1,40 +0,0 @@
-# Copyright 2019 Jariullah Safi
-
-# This program is free software: you can redistribute it and/or modify
-# it under the terms of the GNU Lesser General Public License as published by
-# the Free Software Foundation, either version 3 of the License, or
-# (at your option) any later version.
-
-# This program is distributed in the hope that it will be useful,
-# but WITHOUT ANY WARRANTY; without even the implied warranty of
-# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-# GNU Lesser General Public License for more details.
-
-# You should have received a copy of the GNU Lesser General Public License
-# along with this program. If not, see .
-
-cmake_minimum_required(VERSION 3.0.0)
-project(yag_slam)
-
-find_package(catkin QUIET)
-
-if (catkin_FOUND)
- include_directories(include)
- catkin_package(
- INCLUDE_DIRS include
- )
-else()
- include_directories(include)
-endif()
-
-add_subdirectory(pybind11)
-
-pybind11_add_module(yag_slam_cpp src/PythonInterface.cpp src/Impls.cpp src/ScanMatcher.cpp)
-target_link_libraries(yag_slam_cpp)
-
-if (catkin_FOUND)
- install(TARGETS yag_slam_cpp
- LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION}
- )
- install(DIRECTORY yag_slam DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION})
-endif()
\ No newline at end of file
diff --git a/Makefile b/Makefile
deleted file mode 100644
index 507ff87..0000000
--- a/Makefile
+++ /dev/null
@@ -1,3 +0,0 @@
-setup:
- git submodule update --init
- pip install pybind11_cmake
\ No newline at end of file
diff --git a/build.sh b/build.sh
deleted file mode 100755
index 87bb8ca..0000000
--- a/build.sh
+++ /dev/null
@@ -1,28 +0,0 @@
-#!/bin/bash
-
-#docker run --rm -v `pwd`:/repo --rm safijari/manylinux_2_28-x64 /repo/run_build.sh
-
-cd /repo
-
-rm -r build
-rm -r dist
-
-set -e
-
-yum install boost-thread boost-atomic boost-chrono boost-devel -y
-
-/opt/python/cp38-cp38/bin/pip install -U auditwheel
-
-for folder in /opt/python/cp3*
-do
- echo $folder
- $folder/bin/pip install pybind11 pybind11-cmake
- $folder/bin/python setup.py bdist_wheel
-done
-
-cd dist
-
-for file in ./*
-do
- /opt/python/cp38-cp38/bin/auditwheel -v repair --plat $1 $file
-done
diff --git a/include/AdditionalMath.h b/include/AdditionalMath.h
deleted file mode 100644
index 16fb0ba..0000000
--- a/include/AdditionalMath.h
+++ /dev/null
@@ -1,218 +0,0 @@
-/*
- * Copyright 2010 SRI International
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with this program. If not, see .
- */
-
-#ifndef MP_MATH_H
-#define MP_MATH_H
-
-#include
-#include
-#include
-#include
-
-/**
- * Platform independent pi definitions
- */
-const double KT_PI = 3.14159265358979323846; // The value of PI
-const double KT_2PI = 6.28318530717958647692; // 2 * PI
-const double KT_PI_2 = 1.57079632679489661923; // PI / 2
-const double KT_PI_180 = 0.01745329251994329577; // PI / 180
-const double KT_180_PI = 57.29577951308232087685; // 180 / PI
-
-/**
- * Lets define a small number!
- */
-const double KT_TOLERANCE = 1e-06;
-
-/**
- * Lets define max value of int32_t (int32_t) to use it to mark invalid scans
- */
-
-const int32_t INVALID_SCAN = std::numeric_limits::max();
-
-namespace amath {
-/**
- * Converts degrees into radians
- * @param degrees
- * @return radian equivalent of degrees
- */
-inline double DegreesToRadians(double degrees) { return degrees * KT_PI_180; }
-
-/**
- * Converts radians into degrees
- * @param radians
- * @return degree equivalent of radians
- */
-inline double RadiansToDegrees(double radians) { return radians * KT_180_PI; }
-
-/**
- * Square function
- * @param value
- * @return square of value
- */
-template inline T Square(T value) { return (value * value); }
-
-/**
- * Round function
- * @param value
- * @return rounds value to the nearest whole number (as double)
- */
-inline double Round(double value) {
- return value >= 0.0 ? floor(value + 0.5) : ceil(value - 0.5);
-}
-
-/**
- * Binary minimum function
- * @param value1
- * @param value2
- * @return the lesser of value1 and value2
- */
-template
-inline const T &Minimum(const T &value1, const T &value2) {
- return value1 < value2 ? value1 : value2;
-}
-
-/**
- * Binary maximum function
- * @param value1
- * @param value2
- * @return the greater of value1 and value2
- */
-template
-inline const T &Maximum(const T &value1, const T &value2) {
- return value1 > value2 ? value1 : value2;
-}
-
-/**
- * Clips a number to the specified minimum and maximum values.
- * @param n number to be clipped
- * @param minValue minimum value
- * @param maxValue maximum value
- * @return the clipped value
- */
-template
-inline const T &Clip(const T &n, const T &minValue, const T &maxValue) {
- return Minimum(Maximum(n, minValue), maxValue);
-}
-
-/**
- * Checks whether two numbers are equal within a certain tolerance.
- * @param a
- * @param b
- * @return true if a and b differ by at most a certain tolerance.
- */
-inline bool DoubleEqual(double a, double b) {
- double delta = a - b;
- return delta < 0.0 ? delta >= -KT_TOLERANCE : delta <= KT_TOLERANCE;
-}
-
-/**
- * Checks whether value is in the range [0;maximum)
- * @param value
- * @param maximum
- */
-template inline bool IsUpTo(const T &value, const T &maximum) {
- return (value >= 0 && value < maximum);
-}
-
-/**
- * Checks whether value is in the range [0;maximum)
- * Specialized version for unsigned int (uint32_t)
- * @param value
- * @param maximum
- */
-template <>
-inline bool IsUpTo(const uint32_t &value, const uint32_t &maximum) {
- return (value < maximum);
-}
-
-/**
- * Checks whether value is in the range [a;b]
- * @param value
- * @param a
- * @param b
- */
-template
-inline bool InRange(const T &value, const T &a, const T &b) {
- return (value >= a && value <= b);
-}
-
-
-template
-inline bool InRange(T &value, T &a, T &b) {
- return (value >= a && value <= b);
-}
-
-/**
- * Normalizes angle to be in the range of [-pi, pi]
- * @param angle to be normalized
- * @return normalized angle
- */
-inline double NormalizeAngle(double angle) {
- while (angle < -KT_PI) {
- if (angle < -KT_2PI) {
- angle += (uint32_t)(angle / -KT_2PI) * KT_2PI;
- } else {
- angle += KT_2PI;
- }
- }
-
- while (angle > KT_PI) {
- if (angle > KT_2PI) {
- angle -= (uint32_t)(angle / KT_2PI) * KT_2PI;
- } else {
- angle -= KT_2PI;
- }
- }
-
- assert(amath::InRange(angle, -KT_PI, KT_PI));
-
- return angle;
-}
-
-/**
- * Returns an equivalent angle to the first parameter such that the difference
- * when the second parameter is subtracted from this new value is an angle
- * in the normalized range of [-pi, pi], i.e. abs(minuend - subtrahend) <= pi.
- * @param minuend
- * @param subtrahend
- * @return normalized angle
- */
-inline double NormalizeAngleDifference(double minuend, double subtrahend) {
- while (minuend - subtrahend < -KT_PI) {
- minuend += KT_2PI;
- }
-
- while (minuend - subtrahend > KT_PI) {
- minuend -= KT_2PI;
- }
-
- return minuend;
-}
-
-/**
- * Align a value to the alignValue.
- * The alignValue should be the power of two (2, 4, 8, 16, 32 and so on)
- * @param value
- * @param alignValue
- * @return aligned value
- */
-template inline T AlignValue(size_t value, size_t alignValue = 8) {
- return static_cast((value + (alignValue - 1)) & ~(alignValue - 1));
-}
-} // namespace math
-
-#endif // MP_MATH_H
diff --git a/include/BoundingBox2.h b/include/BoundingBox2.h
deleted file mode 100644
index 66db08a..0000000
--- a/include/BoundingBox2.h
+++ /dev/null
@@ -1,82 +0,0 @@
-#ifndef BBOX2_H
-#define BBOX2_H
-
-#include
-#include "Vector2.h"
-#include "Size2.h"
-
-class BoundingBox2 {
-public:
- /*
- * Default constructor
- */
- BoundingBox2()
- : m_Minimum(999999999999999999.99999, 999999999999999999.99999),
- m_Maximum(-999999999999999999.99999, -999999999999999999.99999) {}
-
-public:
- /**
- * Get bounding box minimum
- */
- inline const Vector2 &GetMinimum() const { return m_Minimum; }
-
- /**
- * Set bounding box minimum
- */
- inline void SetMinimum(const Vector2 &mMinimum) {
- m_Minimum = mMinimum;
- }
-
- /**
- * Get bounding box maximum
- */
- inline const Vector2 &GetMaximum() const { return m_Maximum; }
-
- /**
- * Set bounding box maximum
- */
- inline void SetMaximum(const Vector2 &rMaximum) {
- m_Maximum = rMaximum;
- }
-
- /**
- * Get the size of the bounding box
- */
- inline Size2 GetSize() const {
- Vector2 size = m_Maximum - m_Minimum;
-
- return Size2(size.GetX(), size.GetY());
- }
-
- /**
- * Add vector to bounding box
- */
- inline void Add(const Vector2 &rPoint) {
- m_Minimum.MakeFloor(rPoint);
- m_Maximum.MakeCeil(rPoint);
- }
-
- /**
- * Add other bounding box to bounding box
- */
- inline void Add(const BoundingBox2 &rBoundingBox) {
- Add(rBoundingBox.GetMinimum());
- Add(rBoundingBox.GetMaximum());
- }
-
- /**
- * Whether the given point is in the bounds of this box
- * @param rPoint
- * @return in bounds?
- */
- inline bool IsInBounds(const Vector2 &rPoint) const {
- return (amath::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) &&
- amath::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY()));
- }
-
-private:
- Vector2 m_Minimum;
- Vector2 m_Maximum;
-}; // BoundingBox2
-
-#endif
diff --git a/include/CoordinateConverter.h b/include/CoordinateConverter.h
deleted file mode 100644
index cfa6f01..0000000
--- a/include/CoordinateConverter.h
+++ /dev/null
@@ -1,142 +0,0 @@
-#ifndef COORD_CVTR_H
-#define COORD_CVTR_H
-
-#include "BoundingBox2.h"
-#include "AdditionalMath.h"
-
-/**
- * The CoordinateConverter class is used to convert coordinates between world
- * and grid coordinates In world coordinates 1.0 = 1 meter where 1 in grid
- * coordinates = 1 pixel! Default scale for coordinate converter is 20 that
- * converters to 1 pixel = 0.05 meter
- */
-class CoordinateConverter {
-public:
- /**
- * Default constructor
- */
- CoordinateConverter() : m_Scale(20.0) {}
-
-public:
- /**
- * Scales the value
- * @param value
- * @return scaled value
- */
- inline double Transform(double value) { return value * m_Scale; }
-
- /**
- * Converts the point from world coordinates to grid coordinates
- * @param rWorld world coordinate
- * @param flipY
- * @return grid coordinate
- */
- inline Vector2 WorldToGrid(const Vector2 &rWorld,
- bool flipY = false) const {
- double gridX = (rWorld.GetX() - m_Offset.GetX()) * m_Scale;
- double gridY = 0.0;
-
- if (flipY == false) {
- gridY = (rWorld.GetY() - m_Offset.GetY()) * m_Scale;
- } else {
- gridY = (m_Size.GetHeight() / m_Scale - rWorld.GetY() + m_Offset.GetY()) *
- m_Scale;
- }
-
- return Vector2(static_cast(amath::Round(gridX)),
- static_cast(amath::Round(gridY)));
- }
-
- /**
- * Converts the point from grid coordinates to world coordinates
- * @param rGrid world coordinate
- * @param flipY
- * @return world coordinate
- */
- inline Vector2 GridToWorld(const Vector2 &rGrid,
- bool flipY = false) const {
- double worldX = m_Offset.GetX() + rGrid.GetX() / m_Scale;
- double worldY = 0.0;
-
- if (flipY == false) {
- worldY = m_Offset.GetY() + rGrid.GetY() / m_Scale;
- } else {
- worldY = m_Offset.GetY() + (m_Size.GetHeight() - rGrid.GetY()) / m_Scale;
- }
-
- return Vector2(worldX, worldY);
- }
-
- /**
- * Gets the scale
- * @return scale
- */
- inline double GetScale() const { return m_Scale; }
-
- /**
- * Sets the scale
- * @param scale
- */
- inline void SetScale(double scale) { m_Scale = scale; }
-
- /**
- * Gets the offset
- * @return offset
- */
- inline const Vector2 &GetOffset() const { return m_Offset; }
-
- /**
- * Sets the offset
- * @param rOffset
- */
- inline void SetOffset(const Vector2 &rOffset) { m_Offset = rOffset; }
-
- /**
- * Sets the size
- * @param rSize
- */
- inline void SetSize(const Size2 &rSize) { m_Size = rSize; }
-
- /**
- * Gets the size
- * @return size
- */
- inline const Size2 &GetSize() const { return m_Size; }
-
- /**
- * Gets the resolution
- * @return resolution
- */
- inline double GetResolution() const { return 1.0 / m_Scale; }
-
- /**
- * Sets the resolution
- * @param resolution
- */
- inline void SetResolution(double resolution) { m_Scale = 1.0 / resolution; }
-
- /**
- * Gets the bounding box
- * @return bounding box
- */
- inline BoundingBox2 GetBoundingBox() const {
- BoundingBox2 box;
-
- double minX = GetOffset().GetX();
- double minY = GetOffset().GetY();
- double maxX = minX + GetSize().GetWidth() * GetResolution();
- double maxY = minY + GetSize().GetHeight() * GetResolution();
-
- box.SetMinimum(GetOffset());
- box.SetMaximum(Vector2(maxX, maxY));
- return box;
- }
-
-private:
- Size2 m_Size;
- double m_Scale;
-
- Vector2 m_Offset;
-}; // CoordinateConverter
-
-#endif
diff --git a/include/CorrelationGrid.h b/include/CorrelationGrid.h
deleted file mode 100644
index ea7672e..0000000
--- a/include/CorrelationGrid.h
+++ /dev/null
@@ -1,209 +0,0 @@
-#ifndef CORRELATION_GRID_H
-#define CORRELATION_GRID_H
-
-#include "Grid.h"
-#include "Rectangle2.h"
-
-class CorrelationGrid : public Grid {
-public:
- /**
- * Destructor
- */
- virtual ~CorrelationGrid() { delete[] m_pKernel; }
-
-public:
- /**
- * Create a correlation grid of given size and parameters
- * @param width
- * @param height
- * @param resolution
- * @param smearDeviation
- * @return correlation grid
- */
- static CorrelationGrid *CreateGrid(int32_t width, int32_t height,
- double resolution, double smearDeviation) {
- assert(resolution != 0.0);
-
- // +1 in case of roundoff
- uint32_t borderSize = GetHalfKernelSize(smearDeviation, resolution) + 1;
-
- CorrelationGrid *pGrid = new CorrelationGrid(width, height, borderSize,
- resolution, smearDeviation);
-
- return pGrid;
- }
-
- /**
- * Gets the index into the data pointer of the given grid coordinate
- * @param rGrid
- * @param boundaryCheck
- * @return grid index
- */
- virtual int32_t GridIndex(const Vector2 &rGrid,
- bool boundaryCheck = true) const {
- int32_t x = rGrid.GetX() + m_Roi.GetX();
- int32_t y = rGrid.GetY() + m_Roi.GetY();
-
- return Grid::GridIndex(Vector2(x, y), boundaryCheck);
- }
-
- /**
- * Get the Region Of Interest (ROI)
- * @return region of interest
- */
- inline const Rectangle2 &GetROI() const { return m_Roi; }
-
- /**
- * Sets the Region Of Interest (ROI)
- * @param roi
- */
- inline void SetROI(const Rectangle2 &roi) { m_Roi = roi; }
-
- /**
- * Smear cell if the cell at the given point is marked as "occupied"
- * @param rGridPoint
- */
- inline void SmearPoint(const Vector2 &rGridPoint) {
- assert(m_pKernel != NULL);
-
- int gridIndex = GridIndex(rGridPoint);
- if (GetDataPointer()[gridIndex] != GridStates_Occupied) {
- return;
- }
-
- int32_t halfKernel = m_KernelSize / 2;
-
- // apply kernel
- for (int32_t j = -halfKernel; j <= halfKernel; j++) {
- uint8_t *pGridAdr = GetDataPointer(
- Vector2(rGridPoint.GetX(), rGridPoint.GetY() + j));
-
- int32_t kernelConstant = (halfKernel) + m_KernelSize * (j + halfKernel);
-
- // if a point is on the edge of the grid, there is no problem
- // with running over the edge of allowable memory, because
- // the grid has margins to compensate for the kernel size
- for (int32_t i = -halfKernel; i <= halfKernel; i++) {
- int32_t kernelArrayIndex = i + kernelConstant;
-
- uint8_t kernelValue = m_pKernel[kernelArrayIndex];
- if (kernelValue > pGridAdr[i]) {
- // kernel value is greater, so set it to kernel value
- pGridAdr[i] = kernelValue;
- }
- }
- }
- }
-
-protected:
- /**
- * Constructs a correlation grid of given size and parameters
- * @param width
- * @param height
- * @param borderSize
- * @param resolution
- * @param smearDeviation
- */
- CorrelationGrid(uint32_t width, uint32_t height, uint32_t borderSize,
- double resolution, double smearDeviation)
- : Grid(width + borderSize * 2, height + borderSize * 2),
- m_SmearDeviation(smearDeviation), m_pKernel(NULL) {
- GetCoordinateConverter()->SetScale(1.0 / resolution);
-
- // setup region of interest
- m_Roi = Rectangle2(borderSize, borderSize, width, height);
-
- // calculate kernel
- CalculateKernel();
- }
-
- /**
- * Sets up the kernel for grid smearing.
- */
- virtual void CalculateKernel() {
- double resolution = GetResolution();
-
- assert(resolution != 0.0);
- assert(m_SmearDeviation != 0.0);
-
- // min and max distance deviation for smearing;
- // will smear for two standard deviations, so deviation must be at least 1/2
- // of the resolution
- const double MIN_SMEAR_DISTANCE_DEVIATION = 0.5 * resolution;
- const double MAX_SMEAR_DISTANCE_DEVIATION = 10 * resolution;
-
- // check if given value too small or too big
- if (!amath::InRange(m_SmearDeviation, MIN_SMEAR_DISTANCE_DEVIATION,
- MAX_SMEAR_DISTANCE_DEVIATION)) {
- std::stringstream error;
- error << "Mapper Error: Smear deviation too small: Must be between "
- << MIN_SMEAR_DISTANCE_DEVIATION << " and "
- << MAX_SMEAR_DISTANCE_DEVIATION;
- throw std::runtime_error(error.str());
- }
-
- // NOTE: Currently assumes a two-dimensional kernel
-
- // +1 for center
- m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1;
-
- // allocate kernel
- m_pKernel = new uint8_t[m_KernelSize * m_KernelSize];
- if (m_pKernel == NULL) {
- throw std::runtime_error("Unable to allocate memory for kernel!");
- }
-
- // calculate kernel
- int32_t halfKernel = m_KernelSize / 2;
- for (int32_t i = -halfKernel; i <= halfKernel; i++) {
- for (int32_t j = -halfKernel; j <= halfKernel; j++) {
-#ifdef WIN32
- double distanceFromMean = _hypot(i * resolution, j * resolution);
-#else
- double distanceFromMean = hypot(i * resolution, j * resolution);
-#endif
- double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2));
-
- uint32_t kernelValue =
- static_cast(amath::Round(z * GridStates_Occupied));
- assert(amath::IsUpTo(kernelValue, static_cast(255)));
-
- int kernelArrayIndex =
- (i + halfKernel) + m_KernelSize * (j + halfKernel);
- m_pKernel[kernelArrayIndex] = static_cast(kernelValue);
- }
- }
- }
-
- /**
- * Computes the kernel half-size based on the smear distance and the grid
- * resolution. Computes to two standard deviations to get 95% region and to
- * reduce aliasing.
- * @param smearDeviation
- * @param resolution
- * @return kernel half-size based on the parameters
- */
- static int32_t GetHalfKernelSize(double smearDeviation, double resolution) {
- assert(resolution != 0.0);
-
- return static_cast(amath::Round(2.0 * smearDeviation / resolution));
- }
-
-private:
- /**
- * The point readings are smeared by this value in X and Y to create a
- * smoother response. Default value is 0.03 meters.
- */
- double m_SmearDeviation;
-
- // Size of one side of the kernel
- int32_t m_KernelSize;
-
- // Cached kernel for smearing
- uint8_t *m_pKernel;
-
- // region of interest
- Rectangle2 m_Roi;
-}; // CorrelationGrid
-
-#endif
diff --git a/include/CustomData.h b/include/CustomData.h
deleted file mode 100644
index 02b1ff0..0000000
--- a/include/CustomData.h
+++ /dev/null
@@ -1,46 +0,0 @@
-#ifndef CUSTOM_DATA_H
-#define CUSTOM_DATA_H
-#include "Object.h"
-
-class CustomData : public Object
-{
- public:
- /**
- * Constructor
- */
- CustomData()
- : Object()
- {
- }
-
- /**
- * Destructor
- */
- virtual ~CustomData()
- {
- }
-
-public:
- /**
- * Write out this custom data as a string
- * @return string representation of this data object
- */
- virtual const std::string Write() const = 0;
-
- /**
- * Read in this custom data from a string
- * @param rValue string representation of this data object
- */
- virtual void Read(const std::string& rValue) = 0;
-
-private:
- CustomData(const CustomData&);
- const CustomData& operator=(const CustomData&);
-};
-
-/**
- * Type declaration of CustomData vector
- */
-typedef std::vector CustomDataVector;
-
-#endif
diff --git a/include/Functor.h b/include/Functor.h
deleted file mode 100644
index 9d8341f..0000000
--- a/include/Functor.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef FUNCTOR_H
-#define FUNCTOR_H
-
-#include
-
-class Functor {
-public:
- /**
- * Functor function
- */
- virtual void operator()(int32_t){};
-}; // Functor
-
-#endif
diff --git a/include/Grid.h b/include/Grid.h
deleted file mode 100644
index c05d6cc..0000000
--- a/include/Grid.h
+++ /dev/null
@@ -1,358 +0,0 @@
-#ifndef GRID_H
-#define GRID_H
-
-#include
-#include
-#include
-
-#include "AdditionalMath.h"
-#include "Functor.h"
-#include "CoordinateConverter.h"
-
-typedef enum
- {
- GridStates_Unknown = 0,
- GridStates_Occupied = 100,
- GridStates_Free = 255
- } GridStates;
-
-/**
- * Defines a grid class
- */
-template class Grid {
-public:
- /**
- * Creates a grid of given size and resolution
- * @param width
- * @param height
- * @param resolution
- * @return grid pointer
- */
- static Grid *CreateGrid(int32_t width, int32_t height, double resolution) {
- Grid *pGrid = new Grid(width, height);
-
- pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution);
-
- return pGrid;
- }
-
- /**
- * Destructor
- */
- virtual ~Grid() {
- delete[] m_pData;
- delete m_pCoordinateConverter;
- }
-
-public:
- /**
- * Clear out the grid data
- */
- void Clear() { std::memset(m_pData, 0, GetDataSize() * sizeof(T)); }
-
- /**
- * Returns a clone of this grid
- * @return grid clone
- */
- Grid *Clone() {
- Grid *pGrid = CreateGrid(GetWidth(), GetHeight(), GetResolution());
- pGrid->GetCoordinateConverter()->SetOffset(
- GetCoordinateConverter()->GetOffset());
-
- memcpy(pGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
-
- return pGrid;
- }
-
- /**
- * Resizes the grid (deletes all old data)
- * @param width
- * @param height
- */
- virtual void Resize(int32_t width, int32_t height) {
- m_Width = width;
- m_Height = height;
- m_WidthStep = amath::AlignValue(width, 8);
-
- if (m_pData != NULL) {
- delete[] m_pData;
- m_pData = NULL;
- }
-
- try {
- m_pData = new T[GetDataSize()];
-
- if (m_pCoordinateConverter == NULL) {
- m_pCoordinateConverter = new CoordinateConverter();
- }
-
- m_pCoordinateConverter->SetSize(Size2(width, height));
- } catch (...) {
- m_pData = NULL;
-
- m_Width = 0;
- m_Height = 0;
- m_WidthStep = 0;
- }
-
- Clear();
- }
-
- /**
- * Checks whether the given coordinates are valid grid indices
- * @param rGrid
- */
- inline bool IsValidGridIndex(const Vector2 &rGrid) const {
- return (amath::IsUpTo(rGrid.GetX(), m_Width) &&
- amath::IsUpTo(rGrid.GetY(), m_Height));
- }
-
- /**
- * Gets the index into the data pointer of the given grid coordinate
- * @param rGrid
- * @param boundaryCheck default value is true
- * @return grid index
- */
- virtual int32_t GridIndex(const Vector2 &rGrid,
- bool boundaryCheck = true) const {
- if (boundaryCheck == true) {
- if (IsValidGridIndex(rGrid) == false) {
- std::stringstream error;
- error << "Index " << rGrid
- << " out of range. Index must be between [0; " << m_Width
- << ") and [0; " << m_Height << ")";
- std::cout << error.str() << std::endl;
- throw error.str();
- }
- }
-
- int32_t index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep);
-
- if (boundaryCheck == true) {
- assert(amath::IsUpTo(index, GetDataSize()));
- }
-
- return index;
- }
-
- /**
- * Gets the grid coordinate from an index
- * @param index
- * @return grid coordinate
- */
- Vector2 IndexToGrid(int32_t index) const {
- Vector2 grid;
-
- grid.SetY(index / m_WidthStep);
- grid.SetX(index - grid.GetY() * m_WidthStep);
-
- return grid;
- }
-
- /**
- * Converts the point from world coordinates to grid coordinates
- * @param rWorld world coordinate
- * @param flipY
- * @return grid coordinate
- */
- inline Vector2 WorldToGrid(const Vector2 &rWorld,
- bool flipY = false) const {
- return GetCoordinateConverter()->WorldToGrid(rWorld, flipY);
- }
-
- /**
- * Converts the point from grid coordinates to world coordinates
- * @param rGrid world coordinate
- * @param flipY
- * @return world coordinate
- */
- inline Vector2 GridToWorld(const Vector2 &rGrid,
- bool flipY = false) const {
- return GetCoordinateConverter()->GridToWorld(rGrid, flipY);
- }
-
- /**
- * Gets pointer to data at given grid coordinate
- * @param rGrid grid coordinate
- * @return grid point
- */
- T *GetDataPointer(const Vector2 &rGrid) {
- int32_t index = GridIndex(rGrid, true);
- return m_pData + index;
- }
-
- /**
- * Gets pointer to data at given grid coordinate
- * @param rGrid grid coordinate
- * @return grid point
- */
- T *GetDataPointer(const Vector2 &rGrid) const {
- int32_t index = GridIndex(rGrid, true);
- return m_pData + index;
- }
-
- /**
- * Gets the width of the grid
- * @return width of the grid
- */
- inline int32_t GetWidth() const { return m_Width; };
-
- /**
- * Gets the height of the grid
- * @return height of the grid
- */
- inline int32_t GetHeight() const { return m_Height; };
-
- /**
- * Get the size as a Size2
- * @return size of the grid
- */
- inline const Size2 GetSize() const {
- return Size2(m_Width, m_Height);
- }
-
- /**
- * Gets the width step in bytes
- * @return width step
- */
- inline int32_t GetWidthStep() const { return m_WidthStep; }
-
- /**
- * Gets the grid data pointer
- * @return data pointer
- */
- inline T *GetDataPointer() { return m_pData; }
-
- /**
- * Gets const grid data pointer
- * @return data pointer
- */
- inline T *GetDataPointer() const { return m_pData; }
-
- /**
- * Gets the allocated grid size in bytes
- * @return data size
- */
- inline int32_t GetDataSize() const { return m_WidthStep * m_Height; }
-
- /**
- * Get value at given grid coordinate
- * @param rGrid grid coordinate
- * @return value
- */
- inline T GetValue(const Vector2 &rGrid) const {
- int32_t index = GridIndex(rGrid);
- return m_pData[index];
- }
-
- /**
- * Gets the coordinate converter for this grid
- * @return coordinate converter
- */
- inline CoordinateConverter *GetCoordinateConverter() const {
- return m_pCoordinateConverter;
- }
-
- /**
- * Gets the resolution
- * @return resolution
- */
- inline double GetResolution() const {
- return GetCoordinateConverter()->GetResolution();
- }
-
- /**
- * Gets the grids bounding box
- * @return bounding box
- */
- inline BoundingBox2 GetBoundingBox() const {
- return GetCoordinateConverter()->GetBoundingBox();
- }
-
- /**
- * Increments all the grid cells from (x0, y0) to (x1, y1);
- * if applicable, apply f to each cell traced
- * @param x0
- * @param y0
- * @param x1
- * @param y1
- * @param f
- */
- void TraceLine(int32_t x0, int32_t y0, int32_t x1, int32_t y1,
- Functor *f = NULL) {
- bool steep = abs(y1 - y0) > abs(x1 - x0);
- if (steep) {
- std::swap(x0, y0);
- std::swap(x1, y1);
- }
- if (x0 > x1) {
- std::swap(x0, x1);
- std::swap(y0, y1);
- }
-
- int32_t deltaX = x1 - x0;
- int32_t deltaY = abs(y1 - y0);
- int32_t error = 0;
- int32_t ystep;
- int32_t y = y0;
-
- if (y0 < y1) {
- ystep = 1;
- } else {
- ystep = -1;
- }
-
- int32_t pointX;
- int32_t pointY;
- for (int32_t x = x0; x <= x1; x++) {
- if (steep) {
- pointX = y;
- pointY = x;
- } else {
- pointX = x;
- pointY = y;
- }
-
- error += deltaY;
-
- if (2 * error >= deltaX) {
- y += ystep;
- error -= deltaX;
- }
-
- Vector2 gridIndex(pointX, pointY);
- if (IsValidGridIndex(gridIndex)) {
- int32_t index = GridIndex(gridIndex, false);
- T *pGridPointer = GetDataPointer();
- pGridPointer[index]++;
-
- if (f != NULL) {
- (*f)(index);
- }
- }
- }
- }
-
-protected:
- /**
- * Constructs grid of given size
- * @param width
- * @param height
- */
- Grid(int32_t width, int32_t height)
- : m_pData(NULL), m_pCoordinateConverter(NULL) {
- Resize(width, height);
- }
-
-private:
- int32_t m_Width; // width of grid
- int32_t m_Height; // height of grid
- int32_t m_WidthStep; // 8 bit aligned width of grid
- T *m_pData; // grid data
-
- // coordinate converter to convert between world coordinates and grid
- // coordinates
- CoordinateConverter *m_pCoordinateConverter;
-}; // Grid
-
-#endif
diff --git a/include/GridIndexLookup.h b/include/GridIndexLookup.h
deleted file mode 100644
index 56d7425..0000000
--- a/include/GridIndexLookup.h
+++ /dev/null
@@ -1,214 +0,0 @@
-#ifndef GRID_IDX_LKUP_H
-#define GRID_IDX_LKUP_H
-#include "Grid.h"
-#include "LocalizedRangeScan.h"
-#include "LookupArray.h"
-#include "Transform.h"
-#include
-/**
- * Create lookup tables for point readings at varying angles in grid.
- * For each angle, grid indexes are calculated for each range reading.
- * This is to speed up finding best angle/position for a localized range scan
- *
- * Used heavily in mapper and localizer.
- *
- * In the localizer, this is a huge speed up for calculating possible position.
- * For each particle, a probability is calculated. The range scan is the same,
- * but all grid indexes at all possible angles are calculated. So when
- * calculating the particle probability at a specific angle, the index table is
- * used to look up probability in probability grid!
- *
- */
-template class GridIndexLookup {
-public:
- /**
- * Construct a GridIndexLookup with a grid
- * @param pGrid
- */
- GridIndexLookup(Grid *pGrid)
- : m_pGrid(pGrid), m_Capacity(0), m_Size(0), m_ppLookupArray(NULL) {}
-
- /**
- * Destructor
- */
- virtual ~GridIndexLookup() { DestroyArrays(); }
-
-public:
- /**
- * Gets the lookup array for a particular angle index
- * @param index
- * @return lookup array
- */
- const LookupArray *GetLookupArray(int32_t index) const {
- assert(amath::IsUpTo(index, m_Size));
-
- return m_ppLookupArray[index];
- }
-
- /**
- * Get angles
- * @return std::vector& angles
- */
- const std::vector &GetAngles() const { return m_Angles; }
-
- /**
- * Compute lookup table of the points of the given scan for the given angular
- * space
- * @param pScan the scan
- * @param angleCenter
- * @param angleOffset computes lookup arrays for the angles within this offset
- * around angleStart
- * @param angleResolution how fine a granularity to compute lookup arrays in
- * the angular space
- */
- void ComputeOffsets(LocalizedRangeScan *pScan, double angleCenter,
- double angleOffset, double angleResolution) {
- assert(angleOffset > 0.0);
- assert(angleResolution > 0.0);
-
-
-
- int32_t nAngles = static_cast(
- amath::Round(angleOffset * 2.0 / angleResolution) + 1);
-
- SetSize(nAngles);
-
-
- //////////////////////////////////////////////////////
- // convert points into local coordinates of scan pose
-
- const PointVectorDouble &rPointReadings = pScan->GetPointReadings();
-
-
- // compute transform to scan pose
- Transform transform(pScan->GetCorrectedPose());
-
-
- Pose2Vector localPoints;
- for (auto iter : rPointReadings) {
- // do inverse transform to get points in local coordinates
- Pose2 vec = transform.InverseTransformPose(Pose2(iter, 0.0));
- localPoints.push_back(vec);
- }
-
-
-
- //////////////////////////////////////////////////////
- // create lookup array for different angles
- double angle = 0.0;
- double startAngle = angleCenter - angleOffset;
- for (int32_t angleIndex = 0; angleIndex < nAngles; angleIndex++) {
- angle = startAngle + angleIndex * angleResolution;
- ComputeOffsets(angleIndex, angle, localPoints, pScan);
- }
-
- // assert(amath::DoubleEqual(angle, angleCenter + angleOffset));
- }
-
-private:
- /**
- * Compute lookup value of points for given angle
- * @param angleIndex
- * @param angle
- * @param rLocalPoints
- */
- void ComputeOffsets(int32_t angleIndex, double angle,
- const Pose2Vector &rLocalPoints,
- LocalizedRangeScan *pScan) {
- m_ppLookupArray[angleIndex]->SetSize(
- static_cast(rLocalPoints.size()));
- m_Angles.at(angleIndex) = angle;
-
- // set up point array by computing relative offsets to points readings
- // when rotated by given angle
-
- const Vector2 &rGridOffset =
- m_pGrid->GetCoordinateConverter()->GetOffset();
-
- double cosine = cos(angle);
- double sine = sin(angle);
-
- int32_t readingIndex = 0;
-
- int32_t *pAngleIndexPointer =
- m_ppLookupArray[angleIndex]->GetArrayPointer();
-
- double maxRange = pScan->GetMaximumRange();
-
- for (auto iter : rLocalPoints) {
- const Vector2 &rPosition = iter.GetPosition();
-
- if (std::isnan(pScan->GetRangeReadings()[readingIndex]) ||
- std::isinf(pScan->GetRangeReadings()[readingIndex])) {
- pAngleIndexPointer[readingIndex] = INVALID_SCAN;
- readingIndex++;
- continue;
- }
-
- // counterclockwise rotation and that rotation is about the origin (0, 0).
- Vector2 offset;
- offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY());
- offset.SetY(sine * rPosition.GetX() + cosine * rPosition.GetY());
-
- // have to compensate for the grid offset when getting the grid index
- Vector2 gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset);
-
- // use base GridIndex to ignore ROI
- int32_t lookupIndex = m_pGrid->Grid::GridIndex(gridPoint, false);
-
- pAngleIndexPointer[readingIndex] = lookupIndex;
-
- readingIndex++;
- }
- assert(readingIndex == rLocalPoints.size());
- }
-
- /**
- * Sets size of lookup table (resize if not big enough)
- * @param size
- */
- void SetSize(int32_t size) {
- assert(size > 0);
-
- if (size > m_Capacity) {
- if (m_ppLookupArray != NULL) {
- DestroyArrays();
- }
-
- m_Capacity = size;
- m_ppLookupArray = new LookupArray *[m_Capacity];
- for (int32_t i = 0; i < m_Capacity; i++) {
- m_ppLookupArray[i] = new LookupArray();
- }
- }
-
- m_Size = size;
-
- m_Angles.resize(size);
- }
-
- /**
- * Delete the arrays
- */
- void DestroyArrays() {
- for (int32_t i = 0; i < m_Capacity; i++) {
- delete m_ppLookupArray[i];
- }
-
- delete[] m_ppLookupArray;
- m_ppLookupArray = NULL;
- }
-
-private:
- Grid *m_pGrid;
-
- int32_t m_Capacity;
- int32_t m_Size;
-
- LookupArray **m_ppLookupArray;
-
- // for sanity check
- std::vector m_Angles;
-}; // class GridIndexLookup
-
-#endif
diff --git a/include/LocalizedRangeScan.h b/include/LocalizedRangeScan.h
deleted file mode 100644
index 241eda5..0000000
--- a/include/LocalizedRangeScan.h
+++ /dev/null
@@ -1,398 +0,0 @@
-#ifndef LOCALIZED_RANGE_SCAN_H
-#define LOCALIZED_RANGE_SCAN_H
-
-#include
-#include
-#include
-
-#include "Transform.h"
-#include "Vector2.h"
-#include "AdditionalMath.h"
-#include "CoordinateConverter.h"
-#include "AdditionalMath.h"
-
-#include
-#include
-#include
-
-#include "Name.h"
-#include "SensorData.h"
-#include "LocalizedRangeScan.h"
-
-/**
- * Type declaration of range readings vector
- */
-typedef std::vector RangeReadingsVector;
-
-struct LaserScanConfig {
- double minAngle;
- double maxAngle;
- double angularResolution;
- double minRange;
- double maxRange;
- double rangeThreshold;
- std::string sensorName;
-};
-
-/**
- * LaserRangeScan representing the range readings from a laser range finder
- * sensor.
- */
-class LaserRangeScan : public SensorData {
-public:
- /**
- * Constructs a scan from the given sensor with the given readings
- * @param rSensorName
- * @param rRangeReadings
- */
- LaserRangeScan(const LaserScanConfig &_config,
- const RangeReadingsVector &rRangeReadings)
- : SensorData(_config.sensorName), config(_config), m_pRangeReadings(NULL),
- m_NumberOfRangeReadings(0) {
-
- SetRangeReadings(rRangeReadings);
- }
-
- /**
- * Destructor
- */
- virtual ~LaserRangeScan() { delete[] m_pRangeReadings; }
-
-public:
- LaserScanConfig config;
- /**
- * Gets the range readings of this scan
- * @return range readings of this scan
- */
- inline double *GetRangeReadings() const { return m_pRangeReadings; }
-
- inline RangeReadingsVector GetRangeReadingsVector() const {
- return RangeReadingsVector(m_pRangeReadings,
- m_pRangeReadings + m_NumberOfRangeReadings);
- }
-
- /**
- * Sets the range readings for this scan
- * @param rRangeReadings
- */
- inline void SetRangeReadings(const RangeReadingsVector &rRangeReadings) {
- // ignore for now! XXXMAE BUGUBUG 05/21/2010 << TODO(lucbettaieb): What the
- // heck is this?? if (rRangeReadings.size() != GetNumberOfRangeReadings())
- // {
- // std::stringstream error;
- // error << "Given number of readings (" << rRangeReadings.size()
- // << ") does not match expected number of range finder (" <<
- // GetNumberOfRangeReadings() << ")";
- // throw Exception(error.str());
- // }
-
- if (!rRangeReadings.empty()) {
- if (rRangeReadings.size() != m_NumberOfRangeReadings) {
- // delete old readings
- delete[] m_pRangeReadings;
-
- // store size of array!
- m_NumberOfRangeReadings = static_cast(rRangeReadings.size());
-
- // allocate range readings
- m_pRangeReadings = new double[m_NumberOfRangeReadings];
- }
-
- // copy readings
- int32_t index = 0;
- for (auto iter : rRangeReadings) {
- m_pRangeReadings[index++] = iter;
- }
-
- } else {
- delete[] m_pRangeReadings;
- m_pRangeReadings = NULL;
- }
- }
-
- /**
- * Gets the number of range readings
- * @return number of range readings
- */
- inline int32_t GetNumberOfRangeReadings() const {
- return m_NumberOfRangeReadings;
- }
-
- /**
- * Gets this range finder sensor's minimum range
- * @return minimum range
- */
- inline double GetMinimumRange() const { return config.minRange; }
-
- /**
- * Gets this range finder sensor's maximum range
- * @return maximum range
- */
- inline double GetMaximumRange() { return config.maxRange; }
-
- /**
- * Gets the range threshold
- * @return range threshold
- */
- inline double GetRangeThreshold() const { return config.rangeThreshold; }
-
- /**
- * Gets this range finder sensor's minimum angle
- * @return minimum angle
- */
- inline double GetMinimumAngle() const { return config.minAngle; }
-
- /**
- * Gets this range finder sensor's maximum angle
- * @return maximum angle
- */
- inline double GetMaximumAngle() const { return config.maxAngle; }
-
- /**
- * Gets this range finder sensor's angular resolution
- * @return angular resolution
- */
- inline double GetAngularResolution() const { return config.angularResolution; }
-
-private:
- LaserRangeScan(const LaserRangeScan &);
- const LaserRangeScan &operator=(const LaserRangeScan &);
-
-private:
- double *m_pRangeReadings;
- int32_t m_NumberOfRangeReadings;
-}; // LaserRangeScan
-
-/**
- * The LocalizedRangeScan contains range data from a single sweep of a laser
- * range finder sensor in a two-dimensional space and position information. The
- * odometer position is the position reported by the robot when the range data
- * was recorded. The corrected position is the position calculated by the mapper
- * (or localizer)
- */
-class LocalizedRangeScan : public LaserRangeScan {
-public:
- /**
- * Constructs a range scan from the given range finder with the given
- * readings
- */
- LocalizedRangeScan(const LaserScanConfig &_config,
- const RangeReadingsVector &rReadings,
- Pose2 _odomPose, Pose2 _correctedPose, uint32_t _uniqueId, double _scanTime)
- : LaserRangeScan(_config, rReadings), m_IsDirty(true) {
- SetOdometricPose(_odomPose);
- SetCorrectedPose(_correctedPose);
- SetUniqueId(_uniqueId);
- SetTime(_scanTime);
- }
-
- /**
- * Destructor
- */
- virtual ~LocalizedRangeScan() {}
-
-private:
- mutable std::shared_timed_mutex m_Lock;
-
-public:
- /**
- * Gets the odometric pose of this scan
- * @return odometric pose of this scan
- */
- inline const Pose2 &GetOdometricPose() const { return m_OdometricPose; }
-
- /**
- * Sets the odometric pose of this scan
- * @param rPose
- */
- inline void SetOdometricPose(const Pose2 &rPose) { m_OdometricPose = rPose; }
-
- /**
- * Gets the (possibly corrected) robot pose at which this scan was taken. The
- * corrected robot pose of the scan is usually set by an external module such
- * as a localization or mapping module when it is determined that the original
- * pose was incorrect. The external module will set the correct pose based on
- * additional sensor data and any context information it has. If the pose has
- * not been corrected, a call to this method returns the same pose as
- * GetOdometricPose().
- * @return corrected pose
- */
- inline const Pose2 &GetCorrectedPose() const { return m_CorrectedPose; }
-
- /**
- * Moves the scan by moving the robot pose to the given location.
- * @param rPose new pose of the robot of this scan
- */
- inline void SetCorrectedPose(const Pose2 &rPose) {
- m_CorrectedPose = rPose;
-
- m_IsDirty = true;
- }
-
- /**
- * Gets barycenter of point readings
- */
- inline const Pose2 &GetBarycenterPose() const {
- std::shared_lock lock(m_Lock);
- if (m_IsDirty) {
- // throw away constness and do an update!
- lock.unlock();
- std::unique_lock uniqueLock(m_Lock);
- const_cast(this)->Update();
- }
-
- return m_BarycenterPose;
- }
-
- /**
- * Gets the bounding box of this scan
- * @return bounding box of this scan
- */
- inline const BoundingBox2 &GetBoundingBox() const {
- std::shared_lock lock(m_Lock);
- if (m_IsDirty) {
- // throw away constness and do an update!
- lock.unlock();
- std::unique_lock uniqueLock(m_Lock);
- const_cast(this)->Update();
- }
-
- return m_BoundingBox;
- }
-
- /**
- * Get point readings in local coordinates
- */
- inline const PointVectorDouble &
- GetPointReadings(bool wantFiltered = false) const {
- std::shared_lock lock(m_Lock);
- if (m_IsDirty) {
- // throw away constness and do an update!
- lock.unlock();
- std::unique_lock uniqueLock(m_Lock);
- const_cast(this)->Update();
- }
-
- if (wantFiltered == true) {
- return m_PointReadings;
- } else {
- return m_UnfilteredPointReadings;
- }
- }
-
-private:
- /**
- * Compute point readings based on range readings
- * Only range readings within [minimum range; range threshold] are returned
- */
- virtual void Update() {
- if (true) {
- m_PointReadings.clear();
- m_UnfilteredPointReadings.clear();
-
- double rangeThreshold = this->GetRangeThreshold();
- double minimumAngle = this->GetMinimumAngle();
- double angularResolution = this->GetAngularResolution();
- Pose2 scanPose = GetCorrectedPose();
-
- // compute point readings
- Vector2 rangePointsSum;
- uint32_t beamNum = 0;
- for (uint32_t i = 0; i < this->GetNumberOfRangeReadings();
- i++, beamNum++) {
- double rangeReading = GetRangeReadings()[i];
- if (!amath::InRange(rangeReading, this->GetMinimumRange(),
- rangeThreshold)) {
- double angle = scanPose.GetHeading() + minimumAngle +
- beamNum * angularResolution;
-
- Vector2 point;
- point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
- point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
-
- m_UnfilteredPointReadings.push_back(point);
- continue;
- }
-
- double angle =
- scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
-
- Vector2 point;
- point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
- point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
-
- m_PointReadings.push_back(point);
- m_UnfilteredPointReadings.push_back(point);
-
- rangePointsSum += point;
- }
-
- // compute barycenter
- double nPoints = static_cast(m_PointReadings.size());
- if (nPoints != 0.0) {
- Vector2 averagePosition =
- Vector2(rangePointsSum / nPoints);
- m_BarycenterPose = Pose2(averagePosition, 0.0);
- } else {
- m_BarycenterPose = scanPose;
- }
-
- // calculate bounding box of scan
- m_BoundingBox = BoundingBox2();
- m_BoundingBox.Add(scanPose.GetPosition());
- for (auto iter : m_PointReadings) {
- m_BoundingBox.Add(iter);
- }
- }
-
- m_IsDirty = false;
- }
-
-private:
- LocalizedRangeScan(const LocalizedRangeScan &);
- const LocalizedRangeScan &operator=(const LocalizedRangeScan &);
-
-private:
- /**
- * Odometric pose of robot
- */
- Pose2 m_OdometricPose;
-
- /**
- * Corrected pose of robot calculated by mapper (or localizer)
- */
- Pose2 m_CorrectedPose;
-
-protected:
- /**
- * Average of all the point readings
- */
- Pose2 m_BarycenterPose;
-
- /**
- * Vector of point readings
- */
- PointVectorDouble m_PointReadings;
-
- /**
- * Vector of unfiltered point readings
- */
- PointVectorDouble m_UnfilteredPointReadings;
-
- /**
- * Bounding box of localized range scan
- */
- BoundingBox2 m_BoundingBox;
-
- /**
- * Internal flag used to update point readings, barycenter and bounding box
- */
- bool m_IsDirty;
-}; // LocalizedRangeScan
-
-/**
- * Type declaration of LocalizedRangeScan vector
- */
-typedef std::vector LocalizedRangeScanVector;
-
-#endif
diff --git a/include/LookupArray.h b/include/LookupArray.h
deleted file mode 100644
index 6df0e45..0000000
--- a/include/LookupArray.h
+++ /dev/null
@@ -1,95 +0,0 @@
-#ifndef LOOKUPARRAY_H
-#define LOOKUPARRAY_H
-
-#include
-#include
-#include
-
-class LookupArray {
-public:
- /**
- * Constructs lookup array
- */
- LookupArray() : m_pArray(NULL), m_Capacity(0), m_Size(0) {}
-
- /**
- * Destructor
- */
- virtual ~LookupArray() {
- assert(m_pArray != NULL);
-
- delete[] m_pArray;
- m_pArray = NULL;
- }
-
-public:
- /**
- * Clear array
- */
- void Clear() { memset(m_pArray, 0, sizeof(int32_t) * m_Capacity); }
-
- /**
- * Gets size of array
- * @return array size
- */
- int32_t GetSize() const { return m_Size; }
-
- /**
- * Sets size of array (resize if not big enough)
- * @param size
- */
- void SetSize(int32_t size) {
- assert(size != 0);
-
- if (size > m_Capacity) {
- if (m_pArray != NULL) {
- delete[] m_pArray;
- }
- m_Capacity = size;
- m_pArray = new int32_t[m_Capacity];
- }
-
- m_Size = size;
- }
-
- /**
- * Gets reference to value at given index
- * @param index
- * @return reference to value at index
- */
- inline int32_t &operator[](int32_t index) {
- assert(index < m_Size);
-
- return m_pArray[index];
- }
-
- /**
- * Gets value at given index
- * @param index
- * @return value at index
- */
- inline int32_t operator[](int32_t index) const {
- assert(index < m_Size);
-
- return m_pArray[index];
- }
-
- /**
- * Gets array pointer
- * @return array pointer
- */
- inline int32_t *GetArrayPointer() { return m_pArray; }
-
- /**
- * Gets array pointer
- * @return array pointer
- */
- inline int32_t *GetArrayPointer() const { return m_pArray; }
-
-private:
- int32_t *m_pArray;
- int32_t m_Capacity;
- int32_t m_Size;
-}; // LookupArray
-
-#endif
diff --git a/include/Matrix.h b/include/Matrix.h
deleted file mode 100644
index 199c12b..0000000
--- a/include/Matrix.h
+++ /dev/null
@@ -1,378 +0,0 @@
-#ifndef MATRIX_H
-#define MATRIX_H
-
-#include
-#include
-#include
-#include
-#include
-
-#include "AdditionalMath.h"
-#include "Pose.h"
-
-class Matrix3 {
-public:
- /**
- * Default constructor
- */
- Matrix3() { Clear(); }
-
- /**
- * Copy constructor
- */
- inline Matrix3(const Matrix3 &rOther) {
- std::memcpy(m_Matrix, rOther.m_Matrix, 9 * sizeof(double));
- }
-
-public:
- /**
- * Sets this matrix to identity matrix
- */
- void SetToIdentity() {
- std::memset(m_Matrix, 0, 9 * sizeof(double));
-
- for (int32_t i = 0; i < 3; i++) {
- m_Matrix[i][i] = 1.0;
- }
- }
-
- /**
- * Sets this matrix to zero matrix
- */
- void Clear() { memset(m_Matrix, 0, 9 * sizeof(double)); }
-
- /**
- * Sets this matrix to be the rotation matrix of rotation around given axis
- * @param x x-coordinate of axis
- * @param y y-coordinate of axis
- * @param z z-coordinate of axis
- * @param radians amount of rotation
- */
- void FromAxisAngle(double x, double y, double z, const double radians) {
- double cosRadians = cos(radians);
- double sinRadians = sin(radians);
- double oneMinusCos = 1.0 - cosRadians;
-
- double xx = x * x;
- double yy = y * y;
- double zz = z * z;
-
- double xyMCos = x * y * oneMinusCos;
- double xzMCos = x * z * oneMinusCos;
- double yzMCos = y * z * oneMinusCos;
-
- double xSin = x * sinRadians;
- double ySin = y * sinRadians;
- double zSin = z * sinRadians;
-
- m_Matrix[0][0] = xx * oneMinusCos + cosRadians;
- m_Matrix[0][1] = xyMCos - zSin;
- m_Matrix[0][2] = xzMCos + ySin;
-
- m_Matrix[1][0] = xyMCos + zSin;
- m_Matrix[1][1] = yy * oneMinusCos + cosRadians;
- m_Matrix[1][2] = yzMCos - xSin;
-
- m_Matrix[2][0] = xzMCos - ySin;
- m_Matrix[2][1] = yzMCos + xSin;
- m_Matrix[2][2] = zz * oneMinusCos + cosRadians;
- }
-
- /**
- * Returns transposed version of this matrix
- * @return transposed matrix
- */
- Matrix3 Transpose() const {
- Matrix3 transpose;
-
- for (uint32_t row = 0; row < 3; row++) {
- for (uint32_t col = 0; col < 3; col++) {
- transpose.m_Matrix[row][col] = m_Matrix[col][row];
- }
- }
-
- return transpose;
- }
-
- /**
- * Returns the inverse of the matrix
- */
- Matrix3 Inverse() const {
- Matrix3 kInverse = *this;
- bool haveInverse = InverseFast(kInverse, 1e-14);
- if (haveInverse == false) {
- assert(false);
- }
- return kInverse;
- }
-
- /**
- * Internal helper method for inverse matrix calculation
- * This code is lifted from the OgreMatrix3 class!!
- */
- bool InverseFast(Matrix3 &rkInverse,
- double fTolerance = KT_TOLERANCE) const {
- // Invert a 3x3 using cofactors. This is about 8 times faster than
- // the Numerical Recipes code which uses Gaussian elimination.
- rkInverse.m_Matrix[0][0] =
- m_Matrix[1][1] * m_Matrix[2][2] - m_Matrix[1][2] * m_Matrix[2][1];
- rkInverse.m_Matrix[0][1] =
- m_Matrix[0][2] * m_Matrix[2][1] - m_Matrix[0][1] * m_Matrix[2][2];
- rkInverse.m_Matrix[0][2] =
- m_Matrix[0][1] * m_Matrix[1][2] - m_Matrix[0][2] * m_Matrix[1][1];
- rkInverse.m_Matrix[1][0] =
- m_Matrix[1][2] * m_Matrix[2][0] - m_Matrix[1][0] * m_Matrix[2][2];
- rkInverse.m_Matrix[1][1] =
- m_Matrix[0][0] * m_Matrix[2][2] - m_Matrix[0][2] * m_Matrix[2][0];
- rkInverse.m_Matrix[1][2] =
- m_Matrix[0][2] * m_Matrix[1][0] - m_Matrix[0][0] * m_Matrix[1][2];
- rkInverse.m_Matrix[2][0] =
- m_Matrix[1][0] * m_Matrix[2][1] - m_Matrix[1][1] * m_Matrix[2][0];
- rkInverse.m_Matrix[2][1] =
- m_Matrix[0][1] * m_Matrix[2][0] - m_Matrix[0][0] * m_Matrix[2][1];
- rkInverse.m_Matrix[2][2] =
- m_Matrix[0][0] * m_Matrix[1][1] - m_Matrix[0][1] * m_Matrix[1][0];
-
- double fDet = m_Matrix[0][0] * rkInverse.m_Matrix[0][0] +
- m_Matrix[0][1] * rkInverse.m_Matrix[1][0] +
- m_Matrix[0][2] * rkInverse.m_Matrix[2][0];
-
- if (fabs(fDet) <= fTolerance) {
- return false;
- }
-
- double fInvDet = 1.0 / fDet;
- for (size_t row = 0; row < 3; row++) {
- for (size_t col = 0; col < 3; col++) {
- rkInverse.m_Matrix[row][col] *= fInvDet;
- }
- }
-
- return true;
- }
-
- /**
- * Returns a string representation of this matrix
- * @return string representation of this matrix
- */
- inline std::string ToString() const {
- std::stringstream converter;
- converter.precision(std::numeric_limits::digits10);
-
- for (int row = 0; row < 3; row++) {
- for (int col = 0; col < 3; col++) {
- converter << m_Matrix[row][col] << " ";
- }
- }
-
- return converter.str();
- }
-
-public:
- /**
- * Assignment operator
- */
- inline Matrix3 &operator=(const Matrix3 &rOther) {
- std::memcpy(m_Matrix, rOther.m_Matrix, 9 * sizeof(double));
- return *this;
- }
-
- /**
- * Matrix element access, allows use of construct mat(r, c)
- * @param row
- * @param column
- * @return reference to mat(r,c)
- */
- inline double &operator()(uint32_t row, uint32_t column) {
- return m_Matrix[row][column];
- }
-
- /**
- * Read-only matrix element access, allows use of construct mat(r, c)
- * @param row
- * @param column
- * @return mat(r,c)
- */
- inline double operator()(uint32_t row, uint32_t column) const {
- return m_Matrix[row][column];
- }
-
- /**
- * Binary Matrix3 multiplication.
- * @param rOther
- * @return Matrix3 product
- */
- Matrix3 operator*(const Matrix3 &rOther) const {
- Matrix3 product;
-
- for (size_t row = 0; row < 3; row++) {
- for (size_t col = 0; col < 3; col++) {
- product.m_Matrix[row][col] =
- m_Matrix[row][0] * rOther.m_Matrix[0][col] +
- m_Matrix[row][1] * rOther.m_Matrix[1][col] +
- m_Matrix[row][2] * rOther.m_Matrix[2][col];
- }
- }
-
- return product;
- }
-
- /**
- * Matrix3 and Pose2 multiplication - matrix * pose [3x3 * 3x1 = 3x1]
- * @param rPose2
- * @return Pose2 product
- */
- inline Pose2 operator*(const Pose2 &rPose2) const {
- Pose2 pose2;
-
- pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] * rPose2.GetY() +
- m_Matrix[0][2] * rPose2.GetHeading());
- pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] * rPose2.GetY() +
- m_Matrix[1][2] * rPose2.GetHeading());
- pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() +
- m_Matrix[2][1] * rPose2.GetY() +
- m_Matrix[2][2] * rPose2.GetHeading());
-
- return pose2;
- }
-
- /**
- * In place Matrix3 add.
- * @param rkMatrix
- */
- inline void operator+=(const Matrix3 &rkMatrix) {
- for (uint32_t row = 0; row < 3; row++) {
- for (uint32_t col = 0; col < 3; col++) {
- m_Matrix[row][col] += rkMatrix.m_Matrix[row][col];
- }
- }
- }
-
- /**
- * Write Matrix3 onto output stream
- * @param rStream output stream
- * @param rMatrix to write
- */
- friend inline std::ostream &operator<<(std::ostream &rStream,
- const Matrix3 &rMatrix) {
- rStream << rMatrix.ToString();
- return rStream;
- }
-
-// private:
- double m_Matrix[3][3];
-}; // Matrix3
-
-////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////////////////////////////////////
-
-/**
- * Defines a general Matrix class.
- */
-class Matrix {
-public:
- /**
- * Constructs a matrix of size rows x columns
- */
- Matrix(uint32_t rows, uint32_t columns)
- : m_Rows(rows), m_Columns(columns), m_pData(NULL) {
- Allocate();
-
- Clear();
- }
-
- /**
- * Destructor
- */
- virtual ~Matrix() { delete[] m_pData; }
-
-public:
- /**
- * Set all entries to 0
- */
- void Clear() {
- if (m_pData != NULL) {
- memset(m_pData, 0, sizeof(double) * m_Rows * m_Columns);
- }
- }
-
- /**
- * Gets the number of rows of the matrix
- * @return nubmer of rows
- */
- inline uint32_t GetRows() const { return m_Rows; }
-
- /**
- * Gets the number of columns of the matrix
- * @return nubmer of columns
- */
- inline uint32_t GetColumns() const { return m_Columns; }
-
- /**
- * Returns a reference to the entry at (row,column)
- * @param row
- * @param column
- * @return reference to entry at (row,column)
- */
- inline double &operator()(uint32_t row, uint32_t column) {
- RangeCheck(row, column);
-
- return m_pData[row + column * m_Rows];
- }
-
- /**
- * Returns a const reference to the entry at (row,column)
- * @param row
- * @param column
- * @return const reference to entry at (row,column)
- */
- inline const double &operator()(uint32_t row, uint32_t column) const {
- RangeCheck(row, column);
-
- return m_pData[row + column * m_Rows];
- }
-
-private:
- /**
- * Allocate space for the matrix
- */
- void Allocate() {
- try {
- if (m_pData != NULL) {
- delete[] m_pData;
- }
-
- m_pData = new double[m_Rows * m_Columns];
- } catch (const std::bad_alloc &ex) {
- throw "Matrix allocation error";
- }
-
- if (m_pData == NULL) {
- throw "Matrix allocation error";
- }
- }
-
- /**
- * Checks if (row,column) is a valid entry into the matrix
- * @param row
- * @param column
- */
- inline void RangeCheck(uint32_t row, uint32_t column) const {
- if (amath::IsUpTo(row, m_Rows) == false) {
- throw "Matrix - RangeCheck ERROR!!!!";
- }
-
- if (amath::IsUpTo(column, m_Columns) == false) {
- throw "Matrix - RangeCheck ERROR!!!!";
- }
- }
-
-private:
- uint32_t m_Rows;
- uint32_t m_Columns;
-
- double *m_pData;
-}; // Matrix
-
-#endif
diff --git a/include/Name.h b/include/Name.h
deleted file mode 100644
index 2d412c1..0000000
--- a/include/Name.h
+++ /dev/null
@@ -1,187 +0,0 @@
-#ifndef NAME_H
-#define NAME_H
-
-#include
-
-class Name {
-public:
- /**
- * Constructor
- */
- Name() {}
-
- /**
- * Constructor
- */
- Name(const std::string &rName) { Parse(rName); }
-
- /**
- * Constructor
- */
- Name(const Name &rOther) : m_Name(rOther.m_Name), m_Scope(rOther.m_Scope) {}
-
- /**
- * Destructor
- */
- virtual ~Name() {}
-
-public:
- /**
- * Gets the name of this name
- * @return name
- */
- inline const std::string &GetName() const { return m_Name; }
-
- /**
- * Sets the name
- * @param rName name
- */
- inline void SetName(const std::string &rName) {
- std::string::size_type pos = rName.find_last_of('/');
- if (pos != 0 && pos != std::string::npos) {
- throw "Name can't contain a scope!";
- }
-
- m_Name = rName;
- }
-
- /**
- * Gets the scope of this name
- * @return scope
- */
- inline const std::string &GetScope() const { return m_Scope; }
-
- /**
- * Sets the scope of this name
- * @param rScope scope
- */
- inline void SetScope(const std::string &rScope) { m_Scope = rScope; }
-
- /**
- * Returns a string representation of this name
- * @return string representation of this name
- */
- inline std::string ToString() const {
- if (m_Scope == "") {
- return m_Name;
- } else {
- std::string name;
- name.append("/");
- name.append(m_Scope);
- name.append("/");
- name.append(m_Name);
-
- return name;
- }
- }
-
-public:
- /**
- * Assignment operator.
- */
- Name &operator=(const Name &rOther) {
- if (&rOther != this) {
- m_Name = rOther.m_Name;
- m_Scope = rOther.m_Scope;
- }
-
- return *this;
- }
-
- /**
- * Equality operator.
- */
- bool operator==(const Name &rOther) const {
- return (m_Name == rOther.m_Name) && (m_Scope == rOther.m_Scope);
- }
-
- /**
- * Inequality operator.
- */
- bool operator!=(const Name &rOther) const { return !(*this == rOther); }
-
- /**
- * Less than operator.
- */
- bool operator<(const Name &rOther) const {
- return ToString() < rOther.ToString();
- }
-
- /**
- * Write Name onto output stream
- * @param rStream output stream
- * @param rName to write
- */
- friend inline std::ostream &operator<<(std::ostream &rStream,
- const Name &rName) {
- rStream << rName.ToString();
- return rStream;
- }
-
-private:
- /**
- * Parse the given string into a Name object
- * @param rName name
- */
- void Parse(const std::string &rName) {
- std::string::size_type pos = rName.find_last_of('/');
-
- if (pos == std::string::npos) {
- m_Name = rName;
- } else {
- m_Scope = rName.substr(0, pos);
- m_Name = rName.substr(pos + 1, rName.size());
-
- // remove '/' from m_Scope if first!!
- if (m_Scope.size() > 0 && m_Scope[0] == '/') {
- m_Scope = m_Scope.substr(1, m_Scope.size());
- }
- }
- }
-
- /**
- * Validates the given string as a Name
- * @param rName name
- */
- void Validate(const std::string &rName) {
- if (rName.empty()) {
- return;
- }
-
- char c = rName[0];
- if (IsValidFirst(c)) {
- for (size_t i = 1; i < rName.size(); ++i) {
- c = rName[i];
- if (!IsValid(c)) {
- throw "Invalid character in name. Valid characters must be "
- "within the ranges A-Z, a-z, 0-9, '/', '_' and '-'.";
- }
- }
- } else {
- throw "Invalid first character in name. Valid characters must "
- "be within the ranges A-Z, a-z, and '/'.";
- }
- }
-
- /**
- * Whether the character is valid as a first character (alphanumeric or /)
- * @param c character
- * @return true if the character is a valid first character
- */
- inline bool IsValidFirst(char c) { return (isalpha(c) || c == '/'); }
-
- /**
- * Whether the character is a valid character (alphanumeric, /, _, or -)
- * @param c character
- * @return true if the character is valid
- */
- inline bool IsValid(char c) {
- return (isalnum(c) || c == '/' || c == '_' || c == '-');
- }
-
-private:
- std::string m_Name;
- std::string m_Scope;
-};
-
-#endif
diff --git a/include/Object.h b/include/Object.h
deleted file mode 100644
index 7323f31..0000000
--- a/include/Object.h
+++ /dev/null
@@ -1,105 +0,0 @@
-#ifndef OBJECT_H
-#define OBJECT_H
-
-#include "Name.h"
-#include
-
-class NonCopyable {
-private:
- NonCopyable(const NonCopyable &);
- const NonCopyable &operator=(const NonCopyable &);
-
-protected:
- NonCopyable() {}
-
- virtual ~NonCopyable() {}
-}; // class NonCopyable
-
-/**
- * Abstract base class for Karto objects.
- */
-class Object : public NonCopyable {
-public:
- /**
- * Default constructor
- */
- Object();
-
- /**
- * Constructs an object with the given name
- * @param rName
- */
- Object(const Name &rName);
-
- /**
- * Default constructor
- */
- virtual ~Object();
-
-public:
- /**
- * Gets the name of this object
- * @return name
- */
- inline const Name &GetName() const { return m_Name; }
-
- /**
- * Gets the class name of this object
- * @return class name
- */
- // virtual const char *GetClassName() const = 0;
-
- /**
- * Gets the type of this object
- * @return object type
- */
- // virtual kt_objecttype GetObjectType() const = 0;
-
- // /**
- // * Gets the parameter manager of this dataset
- // * @return parameter manager
- // */
- // virtual inline ParameterManager *GetParameterManager() {
- // return m_pParameterManager;
- // }
-
- /**
- * Gets the named parameter
- * @param rName name of parameter
- * @return parameter
- */
- // inline AbstractParameter *GetParameter(const std::string &rName) const {
- // return m_pParameterManager->Get(rName);
- // }
-
- /**
- * Sets the parameter with the given name with the given value
- * @param rName name
- * @param value value
- */
- template
- inline void SetParameter(const std::string &rName, T value);
-
- /**
- * Gets all parameters
- * @return parameters
- */
- // inline const ParameterVector &GetParameters() const {
- // return m_pParameterManager->GetParameterVector();
- // }
-
-private:
- Object(const Object &);
- const Object &operator=(const Object &);
-
-private:
- Name m_Name;
- // ParameterManager *m_pParameterManager;
-};
-
-/**
- * Type declaration of Object vector
- */
-typedef std::vector