Mapper.h

Go to the documentation of this file.

/*
 * 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 <http://www.gnu.org/licenses/>.
 */
 
#ifndef karto_sdk_MAPPER_H
#define karto_sdk_MAPPER_H
 
#include <map>
#include <vector>
#include <unordered_map>
#include <queue>
 
#include <Eigen/Core>
 
#include "tbb/parallel_for.h"
#include "tbb/parallel_do.h"
#include "tbb/blocked_range.h"
#include <algorithm>
#include <chrono>
 
#include <karto_sdk/Karto.h>
 
#include "nanoflann_adaptors.h"
 
namespace karto
{
  // Listener classes
 
  class MapperListener
  {
  public:
    virtual void Info(const std::string& /*rInfo*/) {};
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
    }
  };
  BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperListener)
  class MapperDebugListener
  {
  public:
    virtual void Debug(const std::string& /*rInfo*/) {};
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
    }
  };
  BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperDebugListener)
  class MapperLoopClosureListener : public MapperListener
  {
  public:
    virtual void LoopClosureCheck(const std::string& /*rInfo*/) {};
 
    virtual void BeginLoopClosure(const std::string& /*rInfo*/) {};
 
    virtual void EndLoopClosure(const std::string& /*rInfo*/) {};
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
    }
  };  // MapperLoopClosureListener
  BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperLoopClosureListener)
 
  class EdgeLabel
  {
  public:
    EdgeLabel()
    {
    }
 
    virtual ~EdgeLabel()
    {
    }
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
    }
  };  // EdgeLabel
 
 
  // A LinkInfo object contains the requisite information for the "spring"
  // that links two scans together--the pose difference and the uncertainty
  // (represented by a covariance matrix).
  class LinkInfo : public EdgeLabel
  {
  public:
    LinkInfo()
    {
    }
    LinkInfo(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
    {
      Update(rPose1, rPose2, rCovariance);
    }
 
    virtual ~LinkInfo()
    {
    }
 
  public:
    void Update(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
    {
      m_Pose1 = rPose1;
      m_Pose2 = rPose2;
 
      // transform second pose into the coordinate system of the first pose
      Transform transform(rPose1, Pose2());
      m_PoseDifference = transform.TransformPose(rPose2);
 
      // transform covariance into reference of first pose
      Matrix3 rotationMatrix;
      rotationMatrix.FromAxisAngle(0, 0, 1, -rPose1.GetHeading());
 
      m_Covariance = rotationMatrix * rCovariance * rotationMatrix.Transpose();
    }
 
    inline const Pose2& GetPose1()
    {
      return m_Pose1;
    }
 
    inline const Pose2& GetPose2()
    {
      return m_Pose2;
    }
 
    inline const Pose2& GetPoseDifference()
    {
      return m_PoseDifference;
    }
 
    inline const Matrix3& GetCovariance()
    {
      return m_Covariance;
    }
 
  private:
    Pose2 m_Pose1;
    Pose2 m_Pose2;
    Pose2 m_PoseDifference;
    Matrix3 m_Covariance;
 
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(EdgeLabel);
      ar & BOOST_SERIALIZATION_NVP(m_Pose1);
      ar & BOOST_SERIALIZATION_NVP(m_Pose2);
      ar & BOOST_SERIALIZATION_NVP(m_PoseDifference);
      ar & BOOST_SERIALIZATION_NVP(m_Covariance);
    }
  };  // LinkInfo
 
 
  template<typename T>
  class Edge;
 
  template<typename T>
  class Vertex
  {
    friend class Edge<T>;
 
  public:
    Vertex()
      : m_pObject(NULL), m_Score(1.0)
    {
    }
    Vertex(T* pObject)
      : m_pObject(pObject), m_Score(1.0)
    {
    }
 
    virtual ~Vertex()
    {
    }
 
    inline const std::vector<Edge<T>*>& GetEdges() const
    {
      return m_Edges;
    }
 
    inline void RemoveEdge(const int& idx)
    {
      m_Edges[idx] = NULL;
      m_Edges.erase(m_Edges.begin() + idx);
      return;
    }
 
    inline const double GetScore() const
    {
      return m_Score;
    }
 
    inline void SetScore(const double score)
    {
      m_Score = score;
    }
 
    inline T* GetObject() const
    {
      return m_pObject;
    }
 
    inline void RemoveObject()
    {
      m_pObject = NULL;
    }
 
    std::vector<Vertex<T>*> GetAdjacentVertices() const
    {
      std::vector<Vertex<T>*> vertices;
 
      const_forEach(typename std::vector<Edge<T>*>, &m_Edges)
      {
        Edge<T>* pEdge = *iter;
 
        if (pEdge == NULL)
        {
          continue;
        }
 
        // check both source and target because we have a undirected graph
        if (pEdge->GetSource() != this)
        {
          vertices.push_back(pEdge->GetSource());
        }
 
        if (pEdge->GetTarget() != this)
        {
          vertices.push_back(pEdge->GetTarget());
        }
      }
 
      return vertices;
    }
 
  private:
    inline void AddEdge(Edge<T>* pEdge)
    {
      m_Edges.push_back(pEdge);
    }
 
    T* m_pObject;
    std::vector<Edge<T>*> m_Edges;
    kt_double m_Score;
 
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
      ar & BOOST_SERIALIZATION_NVP(m_pObject);
      ar & BOOST_SERIALIZATION_NVP(m_Edges);
      ar & BOOST_SERIALIZATION_NVP(m_Score);
    }
  };  // Vertex<T>
 
 
  template<typename T>
  class Edge
  {
  public:
    Edge()
      : m_pSource(NULL)
      , m_pTarget(NULL)
      , m_pLabel(NULL)
    {
    }
    Edge(Vertex<T>* pSource, Vertex<T>* pTarget)
      : m_pSource(pSource)
      , m_pTarget(pTarget)
      , m_pLabel(NULL)
    {
      m_pSource->AddEdge(this);
      m_pTarget->AddEdge(this);
    }
 
    virtual ~Edge()
    {
      m_pSource = NULL;
      m_pTarget = NULL;
 
      if (m_pLabel != NULL)
      {
        delete m_pLabel;
        m_pLabel = NULL;
      }
    }
 
  public:
    inline Vertex<T>* GetSource() const
    {
      return m_pSource;
    }
 
    inline Vertex<T>* GetTarget() const
    {
      return m_pTarget;
    }
 
    inline EdgeLabel* GetLabel()
    {
      return m_pLabel;
    }
 
    inline void SetLabel(EdgeLabel* pLabel)
    {
      m_pLabel = pLabel;
    }
 
  private:
    Vertex<T>* m_pSource;
    Vertex<T>* m_pTarget;
    EdgeLabel* m_pLabel;
 
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
      ar & BOOST_SERIALIZATION_NVP(m_pSource);
      ar & BOOST_SERIALIZATION_NVP(m_pTarget);
      ar & BOOST_SERIALIZATION_NVP(m_pLabel);
    }
  };  // class Edge<T>
 
 
  template<typename T>
  class Visitor
  {
  public:
    virtual kt_bool Visit(Vertex<T>* pVertex) = 0;
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
    }
  };  // Visitor<T>
  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Visitor<LocalizedRangeScan>)
 
  template<typename T>
  class Graph;
 
  template<typename T>
  class GraphTraversal
  {
  public:
    GraphTraversal()
    {
    }
    GraphTraversal(Graph<T>* pGraph)
      : m_pGraph(pGraph)
    {
    }
 
    virtual ~GraphTraversal()
    {
    }
 
  public:
 
    virtual std::vector<T*> TraverseForScans(Vertex<T>* pStartVertex, Visitor<T>* pVisitor) = 0;
    virtual std::vector<Vertex<T>*> TraverseForVertices(Vertex<T>* pStartVertex, Visitor<T>* pVisitor) = 0;
 
  protected:
    Graph<T>* m_pGraph;
 
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
      ar & BOOST_SERIALIZATION_NVP(m_pGraph);
    }
  };  // GraphTraversal<T>
  BOOST_SERIALIZATION_ASSUME_ABSTRACT(GraphTraversal<LocalizedRangeScan>)
 
  
 
  template<typename T>
  class Graph
  {
  public:
    typedef std::map<Name, std::map<int, Vertex<T>*> > VertexMap;
 
  public:
    Graph()
    {
    }
 
    virtual ~Graph()
    {
      Clear();
    }
 
  public:
    inline void AddVertex(const Name& rName, Vertex<T>* pVertex)
    {
      m_Vertices[rName].insert({pVertex->GetObject()->GetStateId(), pVertex});
    }
 
    inline void RemoveVertex(const Name& rName, const int& idx)
    {
      std::map<int, Vertex<LocalizedRangeScan>* >::iterator it = m_Vertices[rName].find(idx);
      if (it != m_Vertices[rName].end())
      {
        it->second = NULL;
        m_Vertices[rName].erase(it);
      }
      else
      {
        std::cout << "RemoveVertex: Failed to remove vertex " << idx 
          << " because it doesnt exist in m_Vertices." << std::endl;
      }
    }
 
    inline void AddEdge(Edge<T>* pEdge)
    {
      m_Edges.push_back(pEdge);
    }
 
    inline void RemoveEdge(const int& idx)
    {
      m_Edges[idx] = NULL;
      m_Edges.erase(m_Edges.begin() + idx);
    }
 
 
    void Clear()
    {
      forEachAs(typename VertexMap, &m_Vertices, indexIter)
      {
        // delete each vertex
        typename std::map<int, Vertex<T>*>::iterator iter;
        for (iter = indexIter->second.begin(); iter != indexIter->second.end(); ++iter)
        {
          delete iter->second;
          iter->second = nullptr;
        }
      }
      m_Vertices.clear();
 
      forEach(typename std::vector<Edge<T>*>, &m_Edges)
      {
        delete *iter;
        *iter = nullptr;
      }
      m_Edges.clear();
    }
 
    inline const std::vector<Edge<T>*>& GetEdges() const
    {
      return m_Edges;
    }
 
    inline const VertexMap& GetVertices() const
    {
      return m_Vertices;
    }
 
  protected:
    VertexMap m_Vertices;
 
    std::vector<Edge<T>*> m_Edges;
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
      std::cout << "Graph <- m_Edges; ";
      ar & BOOST_SERIALIZATION_NVP(m_Edges);
      std::cout << "Graph <- m_Vertices\n";
      ar & BOOST_SERIALIZATION_NVP(m_Vertices);
    }
  };  // Graph<T>
 
 
  class Mapper;
  class ScanMatcher;
 
  class KARTO_EXPORT MapperGraph : public Graph<LocalizedRangeScan>
  {
  public:
    MapperGraph(Mapper* pMapper, kt_double rangeThreshold);
    MapperGraph()
    {
    }
    virtual ~MapperGraph();
 
  public:
    Vertex<LocalizedRangeScan>* AddVertex(LocalizedRangeScan* pScan);
 
    Edge<LocalizedRangeScan>* AddEdge(LocalizedRangeScan* pSourceScan,
                                      LocalizedRangeScan* pTargetScan,
                                      kt_bool& rIsNewEdge);
 
    void AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance);
 
    kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName);
 
    void CorrectPoses();
 
    LocalizedRangeScanVector FindNearLinkedScans(LocalizedRangeScan* pScan, kt_double maxDistance);
 
    std::vector<Vertex<LocalizedRangeScan>*> FindNearLinkedVertices(LocalizedRangeScan* pScan, kt_double maxDistance);
 
    LocalizedRangeScanVector FindNearByScans(Name name, const Pose2 refPose, kt_double maxDistance);
 
    std::vector<Vertex<LocalizedRangeScan>*> FindNearByVertices(Name name, const Pose2 refPose, kt_double maxDistance);
 
    Vertex<LocalizedRangeScan>* FindNearByScan(Name name, const Pose2 refPose);
 
    inline ScanMatcher* GetLoopScanMatcher() const
    {
      return m_pLoopScanMatcher;
    }
 
    void UpdateLoopScanMatcher(kt_double rangeThreshold);
 
  private:
    inline Vertex<LocalizedRangeScan>* GetVertex(LocalizedRangeScan* pScan)
    {
      Name rName = pScan->GetSensorName();
      std::map<int, Vertex<LocalizedRangeScan>* >::iterator it = m_Vertices[rName].find(pScan->GetStateId());
      if (it != m_Vertices[rName].end())
      {
        return it->second;
      }
      else
      {
        std::cout << "GetVertex: Failed to get vertex, idx " << pScan->GetStateId() << 
          " is not in m_Vertices." << std::endl;
        return nullptr;
      }
    }
 
    LocalizedRangeScan* GetClosestScanToPose(const LocalizedRangeScanVector& rScans, const Pose2& rPose) const;
 
    void LinkScans(LocalizedRangeScan* pFromScan,
                   LocalizedRangeScan* pToScan,
                   const Pose2& rMean,
                   const Matrix3& rCovariance);
 
    void LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector<Matrix3>& rCovariances);
 
    void LinkChainToScan(const LocalizedRangeScanVector& rChain,
                         LocalizedRangeScan* pScan,
                         const Pose2& rMean,
                         const Matrix3& rCovariance);
 
    std::vector<LocalizedRangeScanVector> FindNearChains(LocalizedRangeScan* pScan);
 
    Pose2 ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector<Matrix3>& rCovariances) const;
 
    LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan* pScan,
                                                     const Name& rSensorName,
                                                     kt_int32u& rStartNum);
 
  private:
    Mapper* m_pMapper;
 
    ScanMatcher* m_pLoopScanMatcher;
 
    GraphTraversal<LocalizedRangeScan>* m_pTraversal;
 
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
      std::cout << "MapperGraph <- Graph; ";
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Graph<LocalizedRangeScan>);
      std::cout << "MapperGraph <- m_pMapper; ";
      ar & BOOST_SERIALIZATION_NVP(m_pMapper);
      std::cout << "MapperGraph <- m_pLoopScanMatcher; ";
      ar & BOOST_SERIALIZATION_NVP(m_pLoopScanMatcher);
      std::cout << "MapperGraph <- m_pTraversal\n";
      ar & BOOST_SERIALIZATION_NVP(m_pTraversal);
    }
 
  };  // MapperGraph
 
 
  class ScanSolver
  {
  public:
    typedef std::vector<std::pair<kt_int32s, Pose2> > IdPoseVector;
 
    ScanSolver()
    {
    }
 
    virtual ~ScanSolver()
    {
    }
 
  public:
    virtual void Compute() = 0;
 
    virtual const IdPoseVector& GetCorrections() const = 0;
 
    virtual void AddNode(Vertex<LocalizedRangeScan>* /*pVertex*/)
    {
    }
 
    virtual void RemoveNode(kt_int32s /*id*/)
    {
    }
 
    virtual void AddConstraint(Edge<LocalizedRangeScan>* /*pEdge*/)
    {
    }
 
    virtual void RemoveConstraint(kt_int32s /*sourceId*/, kt_int32s /*targetId*/)
    {
    }
 
    virtual void Clear()
    {
    }
 
    virtual void Reset()
    {
    }
 
    virtual std::unordered_map<int, Eigen::Vector3d>* getGraph()
    {
      std::cout << "getGraph method not implemented for this solver type. Graph visualization unavailable." << std::endl;
      return nullptr;
    }
 
    virtual void ModifyNode(const int& unique_id, Eigen::Vector3d pose)
    {
      std::cout << "ModifyNode method not implemented for this solver type. Manual loop closure unavailable." << std::endl;
    };
    virtual void GetNodeOrientation(const int& unique_id, double& pose)
    {
      std::cout << "GetNodeOrientation method not implemented for this solver type." << std::endl;
    };
 
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
    }
  };  // ScanSolver
  BOOST_SERIALIZATION_ASSUME_ABSTRACT(ScanSolver)
 
  class CorrelationGrid : public Grid<kt_int8u>
  {
  public:
    virtual ~CorrelationGrid()
    {
      if (m_pKernel)
      {
        delete [] m_pKernel;
      }
 
    }
 
  public:
    CorrelationGrid()
    {
    }
    static CorrelationGrid* CreateGrid(kt_int32s width,
                                       kt_int32s height,
                                       kt_double resolution,
                                       kt_double smearDeviation)
    {
      assert(resolution != 0.0);
 
      // +1 in case of roundoff
      kt_int32u borderSize = GetHalfKernelSize(smearDeviation, resolution) + 1;
 
      CorrelationGrid* pGrid = new CorrelationGrid(width, height, borderSize, resolution, smearDeviation);
 
      return pGrid;
    }
 
    virtual kt_int32s GridIndex(const Vector2<kt_int32s>& rGrid, kt_bool boundaryCheck = true) const
    {
      kt_int32s x = rGrid.GetX() + m_Roi.GetX();
      kt_int32s y = rGrid.GetY() + m_Roi.GetY();
 
      return Grid<kt_int8u>::GridIndex(Vector2<kt_int32s>(x, y), boundaryCheck);
    }
 
    inline const Rectangle2<kt_int32s>& GetROI() const
    {
      return m_Roi;
    }
 
    inline void SetROI(const Rectangle2<kt_int32s>& roi)
    {
      m_Roi = roi;
    }
 
    inline void SmearPoint(const Vector2<kt_int32s>& rGridPoint)
    {
      assert(m_pKernel != NULL);
 
      int gridIndex = GridIndex(rGridPoint);
      if (GetDataPointer()[gridIndex] != GridStates_Occupied)
      {
        return;
      }
 
      kt_int32s halfKernel = m_KernelSize / 2;
 
      // apply kernel
      for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
      {
        kt_int8u* pGridAdr = GetDataPointer(Vector2<kt_int32s>(rGridPoint.GetX(), rGridPoint.GetY() + j));
 
        kt_int32s 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 (kt_int32s i = -halfKernel; i <= halfKernel; i++)
        {
          kt_int32s kernelArrayIndex = i + kernelConstant;
 
          kt_int8u kernelValue = m_pKernel[kernelArrayIndex];
          if (kernelValue > pGridAdr[i])
          {
            // kernel value is greater, so set it to kernel value
            pGridAdr[i] = kernelValue;
          }
        }
      }
    }
 
  protected:
    CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize,
                    kt_double resolution, kt_double smearDeviation)
      : Grid<kt_int8u>(width + borderSize * 2, height + borderSize * 2)
      , m_SmearDeviation(smearDeviation)
      , m_pKernel(NULL)
    {
      GetCoordinateConverter()->SetScale(1.0 / resolution);
 
      // setup region of interest
      m_Roi = Rectangle2<kt_int32s>(borderSize, borderSize, width, height);
 
      // calculate kernel
      CalculateKernel();
    }
 
    virtual void CalculateKernel()
    {
      kt_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 kt_double MIN_SMEAR_DISTANCE_DEVIATION = 0.5 * resolution;
      const kt_double MAX_SMEAR_DISTANCE_DEVIATION = 10 * resolution;
 
      // check if given value too small or too big
      if (!math::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 kt_int8u[m_KernelSize * m_KernelSize];
      if (m_pKernel == NULL)
      {
        throw std::runtime_error("Unable to allocate memory for kernel!");
      }
 
      // calculate kernel
      kt_int32s halfKernel = m_KernelSize / 2;
      for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
      {
        for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
        {
#ifdef WIN32
          kt_double distanceFromMean = _hypot(i * resolution, j * resolution);
#else
          kt_double distanceFromMean = hypot(i * resolution, j * resolution);
#endif
          kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2));
 
          kt_int32u kernelValue = static_cast<kt_int32u>(math::Round(z * GridStates_Occupied));
          assert(math::IsUpTo(kernelValue, static_cast<kt_int32u>(255)));
 
          int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel);
          m_pKernel[kernelArrayIndex] = static_cast<kt_int8u>(kernelValue);
        }
      }
    }
 
    static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution)
    {
      assert(resolution != 0.0);
 
      return static_cast<kt_int32s>(math::Round(2.0 * smearDeviation / resolution));
    }
 
  private:
    kt_double m_SmearDeviation;
 
    // Size of one side of the kernel
    kt_int32s m_KernelSize;
 
    // Cached kernel for smearing
    kt_int8u* m_pKernel;
 
    // region of interest
    Rectangle2<kt_int32s> m_Roi;
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Grid<kt_int8u>);
      ar & BOOST_SERIALIZATION_NVP(m_SmearDeviation);
      ar & BOOST_SERIALIZATION_NVP(m_KernelSize);
      if (Archive::is_loading::value)
      {
        m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];
      }
      ar & boost::serialization::make_array<kt_int8u>(m_pKernel, m_KernelSize * m_KernelSize);
      ar & BOOST_SERIALIZATION_NVP(m_Roi);
    }
  };  // CorrelationGrid
  BOOST_SERIALIZATION_ASSUME_ABSTRACT(CorrelationGrid)
 
  class KARTO_EXPORT ScanMatcher
  {
  public:
    ScanMatcher()
    {
    }
    virtual ~ScanMatcher();
 
  public:
    void operator() (const kt_double& y) const;
 
    static ScanMatcher* Create(Mapper* pMapper,
                               kt_double searchSize,
                               kt_double resolution,
                               kt_double smearDeviation,
                               kt_double rangeThreshold);
 
    template<class T = LocalizedRangeScanVector>
    kt_double MatchScan(LocalizedRangeScan* pScan,
                        const T& rBaseScans,
                        Pose2& rMean, Matrix3& rCovariance,
                        kt_bool doPenalize = true,
                        kt_bool doRefineMatch = true);
 
    kt_double CorrelateScan(LocalizedRangeScan* pScan,
                            const Pose2& rSearchCenter,
                            const Vector2<kt_double>& rSearchSpaceOffset,
                            const Vector2<kt_double>& rSearchSpaceResolution,
                            kt_double searchAngleOffset,
                            kt_double searchAngleResolution,
                            kt_bool doPenalize,
                            Pose2& rMean,
                            Matrix3& rCovariance,
                            kt_bool doingFineMatch);
 
    void ComputePositionalCovariance(const Pose2& rBestPose,
                                     kt_double bestResponse,
                                     const Pose2& rSearchCenter,
                                     const Vector2<kt_double>& rSearchSpaceOffset,
                                     const Vector2<kt_double>& rSearchSpaceResolution,
                                     kt_double searchAngleResolution,
                                     Matrix3& rCovariance);
 
    void ComputeAngularCovariance(const Pose2& rBestPose,
                                  kt_double bestResponse,
                                  const Pose2& rSearchCenter,
                                  kt_double searchAngleOffset,
                                  kt_double searchAngleResolution,
                                  Matrix3& rCovariance);
 
    inline CorrelationGrid* GetCorrelationGrid() const
    {
      return m_pCorrelationGrid;
    }
 
  private:
    void AddScans(const LocalizedRangeScanVector& rScans, Vector2<kt_double> viewPoint);
    void AddScans(const LocalizedRangeScanMap& rScans, Vector2<kt_double> viewPoint);
 
    void AddScan(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint, kt_bool doSmear = true);
 
    PointVectorDouble FindValidPoints(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint) const;
 
    kt_double GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const;
 
  protected:
    ScanMatcher(Mapper* pMapper)
      : m_pMapper(pMapper)
      , m_pCorrelationGrid(NULL)
      , m_pSearchSpaceProbs(NULL)
      , m_pGridLookup(NULL)
      , m_pPoseResponse(NULL)
      , m_doPenalize(false)
    {
    }
 
  private:
    Mapper* m_pMapper;
    CorrelationGrid* m_pCorrelationGrid;
    Grid<kt_double>* m_pSearchSpaceProbs;
    GridIndexLookup<kt_int8u>* m_pGridLookup;
    std::pair<kt_double, Pose2>* m_pPoseResponse;
    std::vector<kt_double> m_xPoses;
    std::vector<kt_double> m_yPoses;
    Pose2 m_rSearchCenter;
    kt_double m_searchAngleOffset;
    kt_int32u m_nAngles;
    kt_double m_searchAngleResolution;
    kt_bool m_doPenalize;
 
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
      ar & BOOST_SERIALIZATION_NVP(m_pMapper);
      ar & BOOST_SERIALIZATION_NVP(m_pCorrelationGrid);
      ar & BOOST_SERIALIZATION_NVP(m_pSearchSpaceProbs);
      ar & BOOST_SERIALIZATION_NVP(m_pGridLookup);
      ar & BOOST_SERIALIZATION_NVP(m_xPoses);
      ar & BOOST_SERIALIZATION_NVP(m_yPoses);
      ar & BOOST_SERIALIZATION_NVP(m_rSearchCenter);
      ar & BOOST_SERIALIZATION_NVP(m_searchAngleResolution);
      ar & BOOST_SERIALIZATION_NVP(m_nAngles);
      ar & BOOST_SERIALIZATION_NVP(m_searchAngleResolution);;
      ar & BOOST_SERIALIZATION_NVP(m_doPenalize);
      
      // Note - m_pPoseResponse is generally only ever defined within the
      // execution of ScanMatcher::CorrelateScan and used as a temporary
      // accumulator for multithreaded matching results. It would normally
      // not make sense to serialize, but we don't want to break compatibility
      // with previously serialized data. Gen some dummy data that we free
      // immediately after so that we don't alloc here and leak.
      kt_int32u poseResponseSize =
        static_cast<kt_int32u>(m_xPoses.size() * m_yPoses.size() * m_nAngles);
 
      // We could check first if m_pPoseResponse == nullptr for good measure, but
      // based on the codepaths it should always be freed and set to null outside of
      // any execution of ScanMatcher::CorrelateScan, so go ahead and alloc here.
      m_pPoseResponse = new std::pair<kt_double, Pose2>[poseResponseSize];
      ar & boost::serialization::make_array<std::pair<kt_double, Pose2>>(m_pPoseResponse,
        poseResponseSize);
 
      // Aaaand now, clean up the dummy data
      delete[] m_pPoseResponse;
      m_pPoseResponse = nullptr;
    }
 
  };  // ScanMatcher
 
 
  class ScanManager;
 
  class KARTO_EXPORT MapperSensorManager //: public SensorManager // was commented out, works with it in, but I'll leave out for now...
  {
    typedef std::map<Name, ScanManager*> ScanManagerMap;
 
  public:
    MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
      : m_RunningBufferMaximumSize(runningBufferMaximumSize)
      , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
      , m_NextScanId(0)
    {
    }
 
    MapperSensorManager(){
        }
 
    virtual ~MapperSensorManager()
    {
      Clear();
    }
 
  public:
    void RegisterSensor(const Name& rSensorName);
 
    LocalizedRangeScan* GetScan(const Name& rSensorName, kt_int32s scanIndex);
 
    inline std::vector<Name> GetSensorNames()
    {
      std::vector<Name> deviceNames;
      const_forEach(ScanManagerMap, &m_ScanManagers)
      {
        deviceNames.push_back(iter->first);
      }
 
      return deviceNames;
    }
 
    LocalizedRangeScan* GetLastScan(const Name& rSensorName);
 
    void SetLastScan(LocalizedRangeScan* pScan);
 
    void ClearLastScan(LocalizedRangeScan* pScan);
 
    void ClearLastScan(const Name& name);
 
    inline LocalizedRangeScan* GetScan(kt_int32s id)
    {
      std::map<int, LocalizedRangeScan*>::iterator it = m_Scans.find(id);
      if (it != m_Scans.end())
      {
        return it->second;
      }
      else
      {
        std::cout << "GetScan: id " << id << 
          " does not exist in m_scans, cannot retrieve it." << std::endl;
        return nullptr;
      }
    }
 
    void AddScan(LocalizedRangeScan* pScan);
 
    void AddRunningScan(LocalizedRangeScan* pScan);
 
    void RemoveScan(LocalizedRangeScan* pScan);
 
    LocalizedRangeScanMap& GetScans(const Name& rSensorName);
 
    LocalizedRangeScanVector& GetRunningScans(const Name& rSensorName);
 
    void ClearRunningScans(const Name& rSensorName);
 
    kt_int32u GetRunningScanBufferSize(const Name& rSensorName);
 
    void SetRunningScanBufferSize(kt_int32u rScanBufferSize);
 
    void SetRunningScanBufferMaximumDistance(kt_double rScanBufferMaxDistance);
 
    LocalizedRangeScanVector GetAllScans();
 
    void Clear();
 
  private:
    inline ScanManager* GetScanManager(LocalizedRangeScan* pScan)
    {
      return GetScanManager(pScan->GetSensorName());
    }
 
    inline ScanManager* GetScanManager(const Name& rSensorName)
    {
      if (m_ScanManagers.find(rSensorName) != m_ScanManagers.end())
      {
        return m_ScanManagers[rSensorName];
      }
 
      return NULL;
    }
 
        friend class boost::serialization::access;
        template<class Archive>
        void serialize(Archive &ar, const unsigned int version)
        {
    std::cout << "MapperSensorManager <- m_ScanManagers; ";
    ar & BOOST_SERIALIZATION_NVP(m_ScanManagers);
    ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumSize);
    ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumDistance);
    ar & BOOST_SERIALIZATION_NVP(m_NextScanId);
    std::cout << "MapperSensorManager <- m_Scans\n";
    ar & BOOST_SERIALIZATION_NVP(m_Scans);
        }
 
  private:
    // map from device ID to scan data
    ScanManagerMap m_ScanManagers;
 
    kt_int32u m_RunningBufferMaximumSize;
    kt_double m_RunningBufferMaximumDistance;
 
    kt_int32s m_NextScanId;
 
    std::map<int, LocalizedRangeScan*> m_Scans;
  };  // MapperSensorManager
 
 
  struct LocalizationScanVertex
  {
    LocalizationScanVertex(){return;};
    LocalizationScanVertex(const LocalizationScanVertex& obj){scan = obj.scan; vertex = obj.vertex;};
    LocalizedRangeScan* scan;
    Vertex<LocalizedRangeScan>* vertex;
  };
 
  typedef std::queue<LocalizationScanVertex> LocalizationScanVertices;
 
  class KARTO_EXPORT Mapper : public Module
  {
    friend class MapperGraph;
    friend class ScanMatcher;
 
  public:
    Mapper();
 
    Mapper(const std::string& rName);
 
    virtual ~Mapper();
 
  public:
    void Initialize(kt_double rangeThreshold);
 
    void SaveToFile(const std::string& filename);
 
    void LoadFromFile(const std::string& filename);
 
    virtual void Reset();
 
    virtual kt_bool Process(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
 
    virtual kt_bool Process(Object* pObject);
 
    // processors
    kt_bool ProcessAtDock(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
    kt_bool ProcessAgainstNode(LocalizedRangeScan * pScan, const int & nodeId, Matrix3 * covariance = nullptr);
    kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan* pScan, kt_bool addScanToLocalizationBuffer = false, Matrix3 * covariance = nullptr);
    kt_bool ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
    kt_bool RemoveNodeFromGraph(Vertex<LocalizedRangeScan>*);
    void AddScanToLocalizationBuffer(LocalizedRangeScan* pScan, Vertex<LocalizedRangeScan>* scan_vertex);
    void ClearLocalizationBuffer();
 
    virtual const LocalizedRangeScanVector GetAllProcessedScans() const;
 
    void AddListener(MapperListener* pListener);
 
    void RemoveListener(MapperListener* pListener);
 
    void SetScanSolver(ScanSolver* pSolver);
 
    ScanSolver* getScanSolver();
 
    virtual MapperGraph* GetGraph() const;
 
    virtual ScanMatcher* GetSequentialScanMatcher() const;
 
    virtual ScanMatcher* GetLoopScanMatcher() const;
 
    inline MapperSensorManager* GetMapperSensorManager() const
    {
      return m_pMapperSensorManager;
    }
 
    inline kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName)
    {
      return m_pGraph->TryCloseLoop(pScan, rSensorName);
    }
 
    inline void CorrectPoses()
    {
      m_pGraph->CorrectPoses();
    }
 
  protected:
    void InitializeParameters();
 
    kt_bool HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const;
 
  public:
    // fire information for listeners!!
 
    void FireInfo(const std::string& rInfo) const;
 
    void FireDebug(const std::string& rInfo) const;
 
    void FireLoopClosureCheck(const std::string& rInfo) const;
 
    void FireBeginLoopClosure(const std::string& rInfo) const;
 
    void FireEndLoopClosure(const std::string& rInfo) const;
 
    // FireRunningScansUpdated
 
    // FireCovarianceCalculated
 
    // FireLoopClosureChain
 
  private:
    Mapper(const Mapper&);
 
    const Mapper& operator=(const Mapper&);
 
  public:
    void SetUseScanMatching(kt_bool val) { m_pUseScanMatching->SetValue(val); }
 
  protected:
    kt_bool m_Initialized;
    kt_bool m_Deserialized;
 
    ScanMatcher* m_pSequentialScanMatcher;
 
    MapperSensorManager* m_pMapperSensorManager;
 
    MapperGraph* m_pGraph;
    ScanSolver* m_pScanOptimizer;
    LocalizationScanVertices m_LocalizationScanVertices;
 
 
    std::vector<MapperListener*> m_Listeners;
 
    Parameter<kt_bool>* m_pUseScanMatching;
 
    Parameter<kt_bool>* m_pUseScanBarycenter;
 
    Parameter<kt_double>* m_pMinimumTimeInterval;
 
    Parameter<kt_double>* m_pMinimumTravelDistance;
 
    Parameter<kt_double>* m_pMinimumTravelHeading;
 
    Parameter<kt_int32u>* m_pScanBufferSize;
 
    Parameter<kt_double>* m_pScanBufferMaximumScanDistance;
 
    Parameter<kt_double>* m_pLinkMatchMinimumResponseFine;
 
    Parameter<kt_double>* m_pLinkScanMaximumDistance;
 
    Parameter<kt_bool>* m_pDoLoopClosing;
 
    Parameter<kt_double>* m_pLoopSearchMaximumDistance;
 
    Parameter<kt_int32u>* m_pLoopMatchMinimumChainSize;
 
    Parameter<kt_double>* m_pLoopMatchMaximumVarianceCoarse;
 
    Parameter<kt_double>* m_pLoopMatchMinimumResponseCoarse;
 
    Parameter<kt_double>* m_pLoopMatchMinimumResponseFine;
 
    //    CorrelationParameters correlationParameters;
 
    Parameter<kt_double>* m_pCorrelationSearchSpaceDimension;
 
    Parameter<kt_double>* m_pCorrelationSearchSpaceResolution;
 
    Parameter<kt_double>* m_pCorrelationSearchSpaceSmearDeviation;
 
 
    //    CorrelationParameters loopCorrelationParameters;
 
    Parameter<kt_double>* m_pLoopSearchSpaceDimension;
 
    Parameter<kt_double>* m_pLoopSearchSpaceResolution;
 
    Parameter<kt_double>* m_pLoopSearchSpaceSmearDeviation;
 
    // ScanMatcherParameters;
 
    // Variance of penalty for deviating from odometry when scan-matching.
    // The penalty is a multiplier (less than 1.0) is a function of the
    // delta of the scan position being tested and the odometric pose
    Parameter<kt_double>* m_pDistanceVariancePenalty;
    Parameter<kt_double>* m_pAngleVariancePenalty;
 
    // The range of angles to search during a coarse search and a finer search
    Parameter<kt_double>* m_pFineSearchAngleOffset;
    Parameter<kt_double>* m_pCoarseSearchAngleOffset;
 
    // Resolution of angles to search during a coarse search
    Parameter<kt_double>* m_pCoarseAngleResolution;
 
    // Minimum value of the penalty multiplier so scores do not
    // become too small
    Parameter<kt_double>* m_pMinimumAnglePenalty;
    Parameter<kt_double>* m_pMinimumDistancePenalty;
 
    // whether to increase the search space if no good matches are initially found
    Parameter<kt_bool>* m_pUseResponseExpansion;
 
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive &ar, const unsigned int version)
    {
      std::cout << "Mapper <- Module\n";
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Module);
      ar & BOOST_SERIALIZATION_NVP(m_Initialized);
      std::cout << "Mapper <- m_pSequentialScanMatcher\n";
      ar & BOOST_SERIALIZATION_NVP(m_pSequentialScanMatcher);
      std::cout << "Mapper <- m_pGraph\n";
      ar & BOOST_SERIALIZATION_NVP(m_pGraph);
      std::cout << "Mapper <- m_pMapperSensorManager\n";
      ar & BOOST_SERIALIZATION_NVP(m_pMapperSensorManager);
      std::cout << "Mapper <- m_Listeners\n";
      ar & BOOST_SERIALIZATION_NVP(m_Listeners);
      ar & BOOST_SERIALIZATION_NVP(m_pUseScanMatching);
      ar & BOOST_SERIALIZATION_NVP(m_pUseScanBarycenter);
      ar & BOOST_SERIALIZATION_NVP(m_pMinimumTimeInterval);
      ar & BOOST_SERIALIZATION_NVP(m_pMinimumTravelDistance);
      ar & BOOST_SERIALIZATION_NVP(m_pMinimumTravelHeading);
      ar & BOOST_SERIALIZATION_NVP(m_pScanBufferSize);
      ar & BOOST_SERIALIZATION_NVP(m_pScanBufferMaximumScanDistance);
      ar & BOOST_SERIALIZATION_NVP(m_pLinkMatchMinimumResponseFine);
      ar & BOOST_SERIALIZATION_NVP(m_pLinkScanMaximumDistance);
      ar & BOOST_SERIALIZATION_NVP(m_pDoLoopClosing);
      ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchMaximumDistance);
      ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumChainSize);
      ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMaximumVarianceCoarse);
      ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumResponseCoarse);
      ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumResponseFine);
      ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceDimension);
      ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceResolution);
      ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceSmearDeviation);
      ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceDimension);
      ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceResolution);
      ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceSmearDeviation);
      ar & BOOST_SERIALIZATION_NVP(m_pDistanceVariancePenalty);
      ar & BOOST_SERIALIZATION_NVP(m_pAngleVariancePenalty);
      ar & BOOST_SERIALIZATION_NVP(m_pFineSearchAngleOffset);
      ar & BOOST_SERIALIZATION_NVP(m_pCoarseSearchAngleOffset);
      ar & BOOST_SERIALIZATION_NVP(m_pCoarseAngleResolution);
      ar & BOOST_SERIALIZATION_NVP(m_pMinimumAnglePenalty);
      ar & BOOST_SERIALIZATION_NVP(m_pMinimumDistancePenalty);
      ar & BOOST_SERIALIZATION_NVP(m_pUseResponseExpansion);
      std::cout << "**Finished serializing Mapper**\n";
    }
  public:
    /* Abstract methods for parameter setters and getters */
 
    /* Getters */
    // General Parameters
    bool getParamUseScanMatching();
    bool getParamUseScanBarycenter();
    double getParamMinimumTimeInterval();
    double getParamMinimumTravelDistance();
    double getParamMinimumTravelHeading();
    int getParamScanBufferSize();
    double getParamScanBufferMaximumScanDistance();
    double getParamLinkMatchMinimumResponseFine();
    double getParamLinkScanMaximumDistance();
    double getParamLoopSearchMaximumDistance();
    bool getParamDoLoopClosing();
    int getParamLoopMatchMinimumChainSize();
    double getParamLoopMatchMaximumVarianceCoarse();
    double getParamLoopMatchMinimumResponseCoarse();
    double getParamLoopMatchMinimumResponseFine();
 
    // Correlation Parameters - Correlation Parameters
    double getParamCorrelationSearchSpaceDimension();
    double getParamCorrelationSearchSpaceResolution();
    double getParamCorrelationSearchSpaceSmearDeviation();
 
    // Correlation Parameters - Loop Closure Parameters
    double getParamLoopSearchSpaceDimension();
    double getParamLoopSearchSpaceResolution();
    double getParamLoopSearchSpaceSmearDeviation();
 
    // Scan Matcher Parameters
    double getParamDistanceVariancePenalty();
    double getParamAngleVariancePenalty();
    double getParamFineSearchAngleOffset();
    double getParamCoarseSearchAngleOffset();
    double getParamCoarseAngleResolution();
    double getParamMinimumAnglePenalty();
    double getParamMinimumDistancePenalty();
    bool getParamUseResponseExpansion();
 
    /* Setters */
    // General Parameters
    void setParamUseScanMatching(bool b);
    void setParamUseScanBarycenter(bool b);
    void setParamMinimumTimeInterval(double d);
    void setParamMinimumTravelDistance(double d);
    void setParamMinimumTravelHeading(double d);
    void setParamScanBufferSize(int i);
    void setParamScanBufferMaximumScanDistance(double d);
    void setParamLinkMatchMinimumResponseFine(double d);
    void setParamLinkScanMaximumDistance(double d);
    void setParamLoopSearchMaximumDistance(double d);
    void setParamDoLoopClosing(bool b);
    void setParamLoopMatchMinimumChainSize(int i);
    void setParamLoopMatchMaximumVarianceCoarse(double d);
    void setParamLoopMatchMinimumResponseCoarse(double d);
    void setParamLoopMatchMinimumResponseFine(double d);
 
    // Correlation Parameters - Correlation Parameters
    void setParamCorrelationSearchSpaceDimension(double d);
    void setParamCorrelationSearchSpaceResolution(double d);
    void setParamCorrelationSearchSpaceSmearDeviation(double d);
 
    // Correlation Parameters - Loop Closure Parameters
    void setParamLoopSearchSpaceDimension(double d);
    void setParamLoopSearchSpaceResolution(double d);
    void setParamLoopSearchSpaceSmearDeviation(double d);
 
    // Scan Matcher Parameters
    void setParamDistanceVariancePenalty(double d);
    void setParamAngleVariancePenalty(double d);
    void setParamFineSearchAngleOffset(double d);
    void setParamCoarseSearchAngleOffset(double d);
    void setParamCoarseAngleResolution(double d);
    void setParamMinimumAnglePenalty(double d);
    void setParamMinimumDistancePenalty(double d);
    void setParamUseResponseExpansion(bool b);
  };
  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Mapper)
}  // namespace karto
 
#endif  // karto_sdk_MAPPER_H