Karto.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 OPEN_KARTO_KARTO_H
#define OPEN_KARTO_KARTO_H
 
#include <string>
#include <fstream>
#include <limits>
#include <algorithm>
#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <stdexcept>
 
#include <math.h>
#include <float.h>
#include <stdio.h>
#include <assert.h>
#include <string.h>
#include <boost/thread.hpp>
 
#ifdef USE_POCO
#include <Poco/Mutex.h>
#endif
 
#include <open_karto/Math.h>
#include <open_karto/Macros.h>
 
#define KARTO_Object(name) \
  virtual const char* GetClassName() const { return #name; } \
  virtual kt_objecttype GetObjectType() const { return ObjectType_##name; }
 
typedef kt_int32u kt_objecttype;
 
const kt_objecttype ObjectType_None                         = 0x00000000;
const kt_objecttype ObjectType_Sensor                       = 0x00001000;
const kt_objecttype ObjectType_SensorData                   = 0x00002000;
const kt_objecttype ObjectType_CustomData                   = 0x00004000;
const kt_objecttype ObjectType_Misc                         = 0x10000000;
 
const kt_objecttype ObjectType_Drive                        = ObjectType_Sensor | 0x01;
const kt_objecttype ObjectType_LaserRangeFinder             = ObjectType_Sensor | 0x02;
const kt_objecttype ObjectType_Camera                       = ObjectType_Sensor | 0x04;
 
const kt_objecttype ObjectType_DrivePose                    = ObjectType_SensorData | 0x01;
const kt_objecttype ObjectType_LaserRangeScan               = ObjectType_SensorData | 0x02;
const kt_objecttype ObjectType_LocalizedRangeScan           = ObjectType_SensorData | 0x04;
const kt_objecttype ObjectType_CameraImage                  = ObjectType_SensorData | 0x08;
const kt_objecttype ObjectType_LocalizedRangeScanWithPoints = ObjectType_SensorData | 0x16;
 
const kt_objecttype ObjectType_Header                       = ObjectType_Misc | 0x01;
const kt_objecttype ObjectType_Parameters                   = ObjectType_Misc | 0x02;
const kt_objecttype ObjectType_DatasetInfo                  = ObjectType_Misc | 0x04;
const kt_objecttype ObjectType_Module                       = ObjectType_Misc | 0x08;
 
namespace karto
{
 
 
  class KARTO_EXPORT Exception
  {
  public:
    Exception(const std::string& rMessage = "Karto Exception", kt_int32s errorCode = 0)
      : m_Message(rMessage)
      , m_ErrorCode(errorCode)
    {
    }
 
    Exception(const Exception& rException)
      : m_Message(rException.m_Message)
      , m_ErrorCode(rException.m_ErrorCode)
    {
    }
 
    virtual ~Exception()
    {
    }
 
  public:
    Exception& operator = (const Exception& rException)
    {
      m_Message = rException.m_Message;
      m_ErrorCode = rException.m_ErrorCode;
 
      return *this;
    }
 
  public:
    const std::string& GetErrorMessage() const
    {
      return m_Message;
    }
 
    kt_int32s GetErrorCode()
    {
      return m_ErrorCode;
    }
 
  public:
    friend KARTO_EXPORT std::ostream& operator << (std::ostream& rStream, Exception& rException);
 
  private:
    std::string m_Message;
    kt_int32s m_ErrorCode;
  };  // class Exception
 
 
  class KARTO_EXPORT NonCopyable
  {
  private:
    NonCopyable(const NonCopyable&);
    const NonCopyable& operator=(const NonCopyable&);
 
  protected:
    NonCopyable()
    {
    }
 
    virtual ~NonCopyable()
    {
    }
  };  // class NonCopyable
 
 
  template <class T>
  class Singleton
  {
  public:
    Singleton()
      : m_pPointer(NULL)
    {
    }
 
    virtual ~Singleton()
    {
      delete m_pPointer;
    }
 
    T* Get()
    {
#ifdef USE_POCO
      Poco::FastMutex::ScopedLock lock(m_Mutex);
#endif
      if (m_pPointer == NULL)
      {
        m_pPointer = new T;
      }
 
      return m_pPointer;
    }
 
  private:
    T* m_pPointer;
 
#ifdef USE_POCO
    Poco::FastMutex m_Mutex;
#endif
 
  private:
    Singleton(const Singleton&);
    const Singleton& operator=(const Singleton&);
  };
 
 
  class KARTO_EXPORT Functor
  {
  public:
    virtual void operator() (kt_int32u) {};
  };  // Functor
 
 
  class AbstractParameter;
 
  typedef std::vector<AbstractParameter*> ParameterVector;
 
  class KARTO_EXPORT ParameterManager : public NonCopyable
  {
  public:
    ParameterManager()
    {
    }
 
    virtual ~ParameterManager()
    {
      Clear();
    }
 
  public:
    void Add(AbstractParameter* pParameter);
 
    AbstractParameter* Get(const std::string& rName)
    {
      if (m_ParameterLookup.find(rName) != m_ParameterLookup.end())
      {
        return m_ParameterLookup[rName];
      }
 
      std::cout << "Unknown parameter: " << rName << std::endl;
 
      return NULL;
    }
 
    void Clear();
 
    inline const ParameterVector& GetParameterVector() const
    {
      return m_Parameters;
    }
 
  public:
    AbstractParameter* operator() (const std::string& rName)
    {
      return Get(rName);
    }
 
  private:
    ParameterManager(const ParameterManager&);
    const ParameterManager& operator=(const ParameterManager&);
 
  private:
    ParameterVector m_Parameters;
    std::map<std::string, AbstractParameter*> m_ParameterLookup;
  };  // ParameterManager
 
 
  // valid names
  // 'Test' -- no scope
  // '/Test' -- no scope will be parsed to 'Test'
  // '/scope/Test' - 'scope' scope and 'Test' name
  // '/scope/name/Test' - 'scope/name' scope and 'Test' name
  //
  class Name
  {
  public:
    Name()
    {
    }
 
    Name(const std::string& rName)
    {
      Parse(rName);
    }
 
    Name(const Name& rOther)
      : m_Name(rOther.m_Name)
      , m_Scope(rOther.m_Scope)
    {
    }
 
    virtual ~Name()
    {
    }
 
  public:
    inline const std::string& GetName() const
    {
      return m_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 Exception("Name can't contain a scope!");
      }
 
      m_Name = rName;
    }
 
    inline const std::string& GetScope() const
    {
      return m_Scope;
    }
 
    inline void SetScope(const std::string& rScope)
    {
      m_Scope = rScope;
    }
 
    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:
    Name& operator = (const Name& rOther)
    {
      if (&rOther != this)
      {
        m_Name = rOther.m_Name;
        m_Scope = rOther.m_Scope;
      }
 
      return *this;
    }
 
    kt_bool operator == (const Name& rOther) const
    {
      return (m_Name == rOther.m_Name) && (m_Scope == rOther.m_Scope);
    }
 
    kt_bool operator != (const Name& rOther) const
    {
      return !(*this == rOther);
    }
 
    kt_bool operator < (const Name& rOther) const
    {
      return ToString() < rOther.ToString();
    }
 
    friend inline std::ostream& operator << (std::ostream& rStream, const Name& rName)
    {
      rStream << rName.ToString();
      return rStream;
    }
 
  private:
    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());
        }
      }
    }
 
    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 Exception("Invalid character in name. Valid characters must be within the ranges A-Z, a-z, 0-9, '/', '_' and '-'.");
          }
        }
      }
      else
      {
        throw Exception("Invalid first character in name. Valid characters must be within the ranges A-Z, a-z, and '/'.");
      }
    }
 
    inline kt_bool IsValidFirst(char c)
    {
      return (isalpha(c) || c == '/');
    }
 
    inline kt_bool IsValid(char c)
    {
      return (isalnum(c) || c == '/' || c == '_' || c == '-');
    }
 
  private:
    std::string m_Name;
    std::string m_Scope;
  };
 
 
  class KARTO_EXPORT Object : public NonCopyable
  {
  public:
    Object();
 
    Object(const Name& rName);
 
    virtual ~Object();
 
  public:
    inline const Name& GetName() const
    {
      return m_Name;
    }
 
    virtual const char* GetClassName() const = 0;
 
    virtual kt_objecttype GetObjectType() const = 0;
 
    virtual inline ParameterManager* GetParameterManager()
    {
      return m_pParameterManager;
    }
 
    inline AbstractParameter* GetParameter(const std::string& rName) const
    {
      return m_pParameterManager->Get(rName);
    }
 
    template<typename T>
    inline void SetParameter(const std::string& rName, T value);
 
    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;
  };
 
  typedef std::vector<Object*> ObjectVector;
 
 
  inline kt_bool IsSensor(Object* pObject)
  {
    return (pObject->GetObjectType() & ObjectType_Sensor) == ObjectType_Sensor;
  }
 
  inline kt_bool IsSensorData(Object* pObject)
  {
    return (pObject->GetObjectType() & ObjectType_SensorData) == ObjectType_SensorData;
  }
 
  inline kt_bool IsLaserRangeFinder(Object* pObject)
  {
    return (pObject->GetObjectType() & ObjectType_LaserRangeFinder) == ObjectType_LaserRangeFinder;
  }
 
  inline kt_bool IsLocalizedRangeScan(Object* pObject)
  {
    return (pObject->GetObjectType() & ObjectType_LocalizedRangeScan) == ObjectType_LocalizedRangeScan;
  }
 
  inline kt_bool IsLocalizedRangeScanWithPoints(Object* pObject)
  {
    return (pObject->GetObjectType() & ObjectType_LocalizedRangeScanWithPoints) == ObjectType_LocalizedRangeScanWithPoints;
  }
 
  inline kt_bool IsParameters(Object* pObject)
  {
    return (pObject->GetObjectType() & ObjectType_Parameters) == ObjectType_Parameters;
  }
 
  inline kt_bool IsDatasetInfo(Object* pObject)
  {
    return (pObject->GetObjectType() & ObjectType_DatasetInfo) == ObjectType_DatasetInfo;
  }
 
 
  class KARTO_EXPORT Module : public Object
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(Module)
    // @endcond
 
  public:
    Module(const std::string& rName);
 
    virtual ~Module();
 
  public:
    virtual void Reset() = 0;
 
    virtual kt_bool Process(karto::Object*)
    {
      return false;
    }
 
  private:
    Module(const Module&);
    const Module& operator=(const Module&);
  };
 
 
  template<typename T>
  class Size2
  {
  public:
    Size2()
      : m_Width(0)
      , m_Height(0)
    {
    }
 
    Size2(T width, T height)
      : m_Width(width)
      , m_Height(height)
    {
    }
 
    Size2(const Size2& rOther)
      : m_Width(rOther.m_Width)
      , m_Height(rOther.m_Height)
    {
    }
 
  public:
    inline const T GetWidth() const
    {
      return m_Width;
    }
 
    inline void SetWidth(T width)
    {
      m_Width = width;
    }
 
    inline const T GetHeight() const
    {
      return m_Height;
    }
 
    inline void SetHeight(T height)
    {
      m_Height = height;
    }
 
    inline Size2& operator = (const Size2& rOther)
    {
      m_Width = rOther.m_Width;
      m_Height = rOther.m_Height;
 
      return(*this);
    }
 
    inline kt_bool operator == (const Size2& rOther) const
    {
      return (m_Width == rOther.m_Width && m_Height == rOther.m_Height);
    }
 
    inline kt_bool operator != (const Size2& rOther) const
    {
      return (m_Width != rOther.m_Width || m_Height != rOther.m_Height);
    }
 
    friend inline std::ostream& operator << (std::ostream& rStream, const Size2& rSize)
    {
      rStream << "(" << rSize.m_Width << ", " << rSize.m_Height << ")";
      return rStream;
    }
 
  private:
    T m_Width;
    T m_Height;
  };  // Size2<T>
 
 
  template<typename T>
  class Vector2
  {
  public:
    Vector2()
    {
      m_Values[0] = 0;
      m_Values[1] = 0;
    }
 
    Vector2(T x, T y)
    {
      m_Values[0] = x;
      m_Values[1] = y;
    }
 
  public:
    inline const T& GetX() const
    {
      return m_Values[0];
    }
 
    inline void SetX(const T& x)
    {
      m_Values[0] = x;
    }
 
    inline const T& GetY() const
    {
      return m_Values[1];
    }
 
    inline void SetY(const T& y)
    {
      m_Values[1] = y;
    }
 
    inline void MakeFloor(const Vector2& rOther)
    {
      if ( rOther.m_Values[0] < m_Values[0] ) m_Values[0] = rOther.m_Values[0];
      if ( rOther.m_Values[1] < m_Values[1] ) m_Values[1] = rOther.m_Values[1];
    }
 
    inline void MakeCeil(const Vector2& rOther)
    {
      if ( rOther.m_Values[0] > m_Values[0] ) m_Values[0] = rOther.m_Values[0];
      if ( rOther.m_Values[1] > m_Values[1] ) m_Values[1] = rOther.m_Values[1];
    }
 
    inline kt_double SquaredLength() const
    {
      return math::Square(m_Values[0]) + math::Square(m_Values[1]);
    }
 
    inline kt_double Length() const
    {
      return sqrt(SquaredLength());
    }
 
    inline kt_double SquaredDistance(const Vector2& rOther) const
    {
      return (*this - rOther).SquaredLength();
    }
 
    inline kt_double Distance(const Vector2& rOther) const
    {
      return sqrt(SquaredDistance(rOther));
    }
 
  public:
    inline void operator += (const Vector2& rOther)
    {
      m_Values[0] += rOther.m_Values[0];
      m_Values[1] += rOther.m_Values[1];
    }
 
    inline void operator -= (const Vector2& rOther)
    {
      m_Values[0] -= rOther.m_Values[0];
      m_Values[1] -= rOther.m_Values[1];
    }
 
    inline const Vector2 operator + (const Vector2& rOther) const
    {
      return Vector2(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1]);
    }
 
    inline const Vector2 operator - (const Vector2& rOther) const
    {
      return Vector2(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1]);
    }
 
    inline void operator /= (T scalar)
    {
      m_Values[0] /= scalar;
      m_Values[1] /= scalar;
    }
 
    inline const Vector2 operator / (T scalar) const
    {
      return Vector2(m_Values[0] / scalar, m_Values[1] / scalar);
    }
 
    inline kt_double operator * (const Vector2& rOther) const
    {
      return m_Values[0] * rOther.m_Values[0] + m_Values[1] * rOther.m_Values[1];
    }
 
    inline const Vector2 operator * (T scalar) const
    {
      return Vector2(m_Values[0] * scalar, m_Values[1] * scalar);
    }
 
    inline const Vector2 operator - (T scalar) const
    {
      return Vector2(m_Values[0] - scalar, m_Values[1] - scalar);
    }
 
    inline void operator *= (T scalar)
    {
      m_Values[0] *= scalar;
      m_Values[1] *= scalar;
    }
 
    inline kt_bool operator == (const Vector2& rOther) const
    {
      return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1]);
    }
 
    inline kt_bool operator != (const Vector2& rOther) const
    {
      return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1]);
    }
 
    inline kt_bool operator < (const Vector2& rOther) const
    {
      if (m_Values[0] < rOther.m_Values[0])
        return true;
      else if (m_Values[0] > rOther.m_Values[0])
        return false;
      else
        return (m_Values[1] < rOther.m_Values[1]);
    }
 
    friend inline std::ostream& operator << (std::ostream& rStream, const Vector2& rVector)
    {
      rStream << rVector.GetX() << " " << rVector.GetY();
      return rStream;
    }
 
    friend inline std::istream& operator >> (std::istream& rStream, const Vector2& /*rVector*/)
    {
      // Implement me!!  TODO(lucbettaieb): What the what?  Do I need to implement this?
      return rStream;
    }
 
  private:
    T m_Values[2];
  };  // Vector2<T>
 
  typedef std::vector< Vector2<kt_double> > PointVectorDouble;
 
 
  template<typename T>
  class Vector3
  {
  public:
    Vector3()
    {
      m_Values[0] = 0;
      m_Values[1] = 0;
      m_Values[2] = 0;
    }
 
    Vector3(T x, T y, T z)
    {
      m_Values[0] = x;
      m_Values[1] = y;
      m_Values[2] = z;
    }
 
    Vector3(const Vector3& rOther)
    {
      m_Values[0] = rOther.m_Values[0];
      m_Values[1] = rOther.m_Values[1];
      m_Values[2] = rOther.m_Values[2];
    }
 
  public:
    inline const T& GetX() const
    {
      return m_Values[0];
    }
 
    inline void SetX(const T& x)
    {
      m_Values[0] = x;
    }
 
    inline const T& GetY() const
    {
      return m_Values[1];
    }
 
    inline void SetY(const T& y)
    {
      m_Values[1] = y;
    }
 
    inline const T& GetZ() const
    {
      return m_Values[2];
    }
 
    inline void SetZ(const T& z)
    {
      m_Values[2] = z;
    }
 
    inline void MakeFloor(const Vector3& rOther)
    {
      if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
      if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
      if (rOther.m_Values[2] < m_Values[2]) m_Values[2] = rOther.m_Values[2];
    }
 
    inline void MakeCeil(const Vector3& rOther)
    {
      if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0];
      if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1];
      if (rOther.m_Values[2] > m_Values[2]) m_Values[2] = rOther.m_Values[2];
    }
 
    inline kt_double SquaredLength() const
    {
      return math::Square(m_Values[0]) + math::Square(m_Values[1]) + math::Square(m_Values[2]);
    }
 
    inline kt_double Length() const
    {
      return sqrt(SquaredLength());
    }
 
    inline std::string ToString() const
    {
      std::stringstream converter;
      converter.precision(std::numeric_limits<double>::digits10);
 
      converter << GetX() << " " << GetY() << " " << GetZ();
 
      return converter.str();
    }
 
  public:
    inline Vector3& operator = (const Vector3& rOther)
    {
      m_Values[0] = rOther.m_Values[0];
      m_Values[1] = rOther.m_Values[1];
      m_Values[2] = rOther.m_Values[2];
 
      return *this;
    }
 
    inline const Vector3 operator + (const Vector3& rOther) const
    {
      return Vector3(m_Values[0] + rOther.m_Values[0],
                     m_Values[1] + rOther.m_Values[1],
                     m_Values[2] + rOther.m_Values[2]);
    }
 
    inline const Vector3 operator + (kt_double scalar) const
    {
      return Vector3(m_Values[0] + scalar,
                     m_Values[1] + scalar,
                     m_Values[2] + scalar);
    }
 
    inline const Vector3 operator - (const Vector3& rOther) const
    {
      return Vector3(m_Values[0] - rOther.m_Values[0],
                     m_Values[1] - rOther.m_Values[1],
                     m_Values[2] - rOther.m_Values[2]);
    }
 
    inline const Vector3 operator - (kt_double scalar) const
    {
      return Vector3(m_Values[0] - scalar, m_Values[1] - scalar, m_Values[2] - scalar);
    }
 
    inline const Vector3 operator * (T scalar) const
    {
      return Vector3(m_Values[0] * scalar, m_Values[1] * scalar, m_Values[2] * scalar);
    }
 
    inline kt_bool operator == (const Vector3& rOther) const
    {
      return (m_Values[0] == rOther.m_Values[0] &&
              m_Values[1] == rOther.m_Values[1] &&
              m_Values[2] == rOther.m_Values[2]);
    }
 
    inline kt_bool operator != (const Vector3& rOther) const
    {
      return (m_Values[0] != rOther.m_Values[0] ||
              m_Values[1] != rOther.m_Values[1] ||
              m_Values[2] != rOther.m_Values[2]);
    }
 
    friend inline std::ostream& operator << (std::ostream& rStream, const Vector3& rVector)
    {
      rStream << rVector.ToString();
      return rStream;
    }
 
    friend inline std::istream& operator >> (std::istream& rStream, const Vector3& /*rVector*/)
    {
      // Implement me!!
      return rStream;
    }
 
  private:
    T m_Values[3];
  };  // Vector3
 
 
  class Quaternion
  {
  public:
    inline Quaternion()
    {
      m_Values[0] = 0.0;
      m_Values[1] = 0.0;
      m_Values[2] = 0.0;
      m_Values[3] = 1.0;
    }
 
    inline Quaternion(kt_double x, kt_double y, kt_double z, kt_double w)
    {
      m_Values[0] = x;
      m_Values[1] = y;
      m_Values[2] = z;
      m_Values[3] = w;
    }
 
    inline Quaternion(const Quaternion& rQuaternion)
    {
      m_Values[0] = rQuaternion.m_Values[0];
      m_Values[1] = rQuaternion.m_Values[1];
      m_Values[2] = rQuaternion.m_Values[2];
      m_Values[3] = rQuaternion.m_Values[3];
    }
 
  public:
    inline kt_double GetX() const
    {
      return m_Values[0];
    }
 
    inline void SetX(kt_double x)
    {
      m_Values[0] = x;
    }
 
    inline kt_double GetY() const
    {
      return m_Values[1];
    }
 
    inline void SetY(kt_double y)
    {
      m_Values[1] = y;
    }
 
    inline kt_double GetZ() const
    {
      return m_Values[2];
    }
 
    inline void SetZ(kt_double z)
    {
      m_Values[2] = z;
    }
 
    inline kt_double GetW() const
    {
      return m_Values[3];
    }
 
    inline void SetW(kt_double w)
    {
      m_Values[3] = w;
    }
 
    void ToEulerAngles(kt_double& rYaw, kt_double& rPitch, kt_double& rRoll) const
    {
      kt_double test = m_Values[0] * m_Values[1] + m_Values[2] * m_Values[3];
 
      if (test > 0.499)
      {
        // singularity at north pole
        rYaw = 2 * atan2(m_Values[0], m_Values[3]);;
        rPitch = KT_PI_2;
        rRoll = 0;
      }
      else if (test < -0.499)
      {
        // singularity at south pole
        rYaw = -2 * atan2(m_Values[0], m_Values[3]);
        rPitch = -KT_PI_2;
        rRoll = 0;
      }
      else
      {
        kt_double sqx = m_Values[0] * m_Values[0];
        kt_double sqy = m_Values[1] * m_Values[1];
        kt_double sqz = m_Values[2] * m_Values[2];
 
        rYaw = atan2(2 * m_Values[1] * m_Values[3] - 2 * m_Values[0] * m_Values[2], 1 - 2 * sqy - 2 * sqz);
        rPitch = asin(2 * test);
        rRoll = atan2(2 * m_Values[0] * m_Values[3] - 2 * m_Values[1] * m_Values[2], 1 - 2 * sqx - 2 * sqz);
      }
    }
 
    void FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll)
    {
      kt_double angle;
 
      angle = yaw * 0.5;
      kt_double cYaw = cos(angle);
      kt_double sYaw = sin(angle);
 
      angle = pitch * 0.5;
      kt_double cPitch = cos(angle);
      kt_double sPitch = sin(angle);
 
      angle = roll * 0.5;
      kt_double cRoll = cos(angle);
      kt_double sRoll = sin(angle);
 
      m_Values[0] = sYaw * sPitch * cRoll + cYaw * cPitch * sRoll;
      m_Values[1] = sYaw * cPitch * cRoll + cYaw * sPitch * sRoll;
      m_Values[2] = cYaw * sPitch * cRoll - sYaw * cPitch * sRoll;
      m_Values[3] = cYaw * cPitch * cRoll - sYaw * sPitch * sRoll;
    }
 
    inline Quaternion& operator = (const Quaternion& rQuaternion)
    {
      m_Values[0] = rQuaternion.m_Values[0];
      m_Values[1] = rQuaternion.m_Values[1];
      m_Values[2] = rQuaternion.m_Values[2];
      m_Values[3] = rQuaternion.m_Values[3];
 
      return(*this);
    }
 
    inline kt_bool operator == (const Quaternion& rOther) const
    {
      return (m_Values[0] == rOther.m_Values[0] &&
              m_Values[1] == rOther.m_Values[1] &&
              m_Values[2] == rOther.m_Values[2] &&
              m_Values[3] == rOther.m_Values[3]);
    }
 
    inline kt_bool operator != (const Quaternion& rOther) const
    {
      return (m_Values[0] != rOther.m_Values[0] ||
              m_Values[1] != rOther.m_Values[1] ||
              m_Values[2] != rOther.m_Values[2] ||
              m_Values[3] != rOther.m_Values[3]);
    }
 
    friend inline std::ostream& operator << (std::ostream& rStream, const Quaternion& rQuaternion)
    {
      rStream << rQuaternion.m_Values[0] << " "
              << rQuaternion.m_Values[1] << " "
              << rQuaternion.m_Values[2] << " "
              << rQuaternion.m_Values[3];
      return rStream;
    }
 
  private:
    kt_double m_Values[4];
  };
 
 
  template<typename T>
  class Rectangle2
  {
  public:
    Rectangle2()
    {
    }
 
    Rectangle2(T x, T y, T width, T height)
      : m_Position(x, y)
      , m_Size(width, height)
    {
    }
 
    Rectangle2(const Vector2<T>& rPosition, const Size2<T>& rSize)
      : m_Position(rPosition)
      , m_Size(rSize)
    {
    }
 
    Rectangle2(const Rectangle2& rOther)
      : m_Position(rOther.m_Position)
      , m_Size(rOther.m_Size)
    {
    }
 
  public:
    inline T GetX() const
    {
      return m_Position.GetX();
    }
 
    inline void SetX(T x)
    {
      m_Position.SetX(x);
    }
 
    inline T GetY() const
    {
      return m_Position.GetY();
    }
 
    inline void SetY(T y)
    {
      m_Position.SetY(y);
    }
 
    inline T GetWidth() const
    {
      return m_Size.GetWidth();
    }
 
    inline void SetWidth(T width)
    {
      m_Size.SetWidth(width);
    }
 
    inline T GetHeight() const
    {
      return m_Size.GetHeight();
    }
 
    inline void SetHeight(T height)
    {
      m_Size.SetHeight(height);
    }
 
    inline const Vector2<T>& GetPosition() const
    {
      return m_Position;
    }
 
    inline void SetPosition(const T& rX, const T& rY)
    {
      m_Position = Vector2<T>(rX, rY);
    }
 
    inline void SetPosition(const Vector2<T>& rPosition)
    {
      m_Position = rPosition;
    }
 
    inline const Size2<T>& GetSize() const
    {
      return m_Size;
    }
 
    inline void SetSize(const Size2<T>& rSize)
    {
      m_Size = rSize;
    }
 
    inline const Vector2<T> GetCenter() const
    {
      return Vector2<T>(m_Position.GetX() + m_Size.GetWidth() * 0.5, m_Position.GetY() + m_Size.GetHeight() * 0.5);
    }
 
  public:
    Rectangle2& operator = (const Rectangle2& rOther)
    {
      m_Position = rOther.m_Position;
      m_Size = rOther.m_Size;
 
      return *this;
    }
 
    inline kt_bool operator == (const Rectangle2& rOther) const
    {
      return (m_Position == rOther.m_Position && m_Size == rOther.m_Size);
    }
 
    inline kt_bool operator != (const Rectangle2& rOther) const
    {
      return (m_Position != rOther.m_Position || m_Size != rOther.m_Size);
    }
 
  private:
    Vector2<T> m_Position;
    Size2<T> m_Size;
  };  // Rectangle2
 
 
  class Pose3;
 
  class Pose2
  {
  public:
    Pose2()
      : m_Heading(0.0)
    {
    }
 
    Pose2(const Vector2<kt_double>& rPosition, kt_double heading)
      : m_Position(rPosition)
      , m_Heading(heading)
    {
    }
 
    Pose2(kt_double x, kt_double y, kt_double heading)
      : m_Position(x, y)
      , m_Heading(heading)
    {
    }
 
    Pose2(const Pose3& rPose);
 
    Pose2(const Pose2& rOther)
      : m_Position(rOther.m_Position)
      , m_Heading(rOther.m_Heading)
    {
    }
 
  public:
    inline kt_double GetX() const
    {
      return m_Position.GetX();
    }
 
    inline void SetX(kt_double x)
    {
      m_Position.SetX(x);
    }
 
    inline kt_double GetY() const
    {
      return m_Position.GetY();
    }
 
    inline void SetY(kt_double y)
    {
      m_Position.SetY(y);
    }
 
    inline const Vector2<kt_double>& GetPosition() const
    {
      return m_Position;
    }
 
    inline void SetPosition(const Vector2<kt_double>& rPosition)
    {
      m_Position = rPosition;
    }
 
    inline kt_double GetHeading() const
    {
      return m_Heading;
    }
 
    inline void SetHeading(kt_double heading)
    {
      m_Heading = heading;
    }
 
    inline kt_double SquaredDistance(const Pose2& rOther) const
    {
      return m_Position.SquaredDistance(rOther.m_Position);
    }
 
  public:
    inline Pose2& operator = (const Pose2& rOther)
    {
      m_Position = rOther.m_Position;
      m_Heading = rOther.m_Heading;
 
      return *this;
    }
 
    inline kt_bool operator == (const Pose2& rOther) const
    {
      return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading);
    }
 
    inline kt_bool operator != (const Pose2& rOther) const
    {
      return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading);
    }
 
    inline void operator += (const Pose2& rOther)
    {
      m_Position += rOther.m_Position;
      m_Heading = math::NormalizeAngle(m_Heading + rOther.m_Heading);
    }
 
    inline Pose2 operator + (const Pose2& rOther) const
    {
      return Pose2(m_Position + rOther.m_Position, math::NormalizeAngle(m_Heading + rOther.m_Heading));
    }
 
    inline Pose2 operator - (const Pose2& rOther) const
    {
      return Pose2(m_Position - rOther.m_Position, math::NormalizeAngle(m_Heading - rOther.m_Heading));
    }
 
    friend inline std::istream& operator >> (std::istream& rStream, const Pose2& /*rPose*/)
    {
      // Implement me!!
      return rStream;
    }
 
    friend inline std::ostream& operator << (std::ostream& rStream, const Pose2& rPose)
    {
      rStream << rPose.m_Position.GetX() << " " << rPose.m_Position.GetY() << " " << rPose.m_Heading;
      return rStream;
    }
 
  private:
    Vector2<kt_double> m_Position;
 
    kt_double m_Heading;
  };  // Pose2
 
  typedef std::vector< Pose2 > Pose2Vector;
 
 
  class Pose3
  {
  public:
    Pose3()
    {
    }
 
    Pose3(const Vector3<kt_double>& rPosition)
      : m_Position(rPosition)
    {
    }
 
    Pose3(const Vector3<kt_double>& rPosition, const karto::Quaternion& rOrientation)
      : m_Position(rPosition)
      , m_Orientation(rOrientation)
    {
    }
 
    Pose3(const Pose3& rOther)
      : m_Position(rOther.m_Position)
      , m_Orientation(rOther.m_Orientation)
    {
    }
 
    Pose3(const Pose2& rPose)
    {
      m_Position = Vector3<kt_double>(rPose.GetX(), rPose.GetY(), 0.0);
      m_Orientation.FromEulerAngles(rPose.GetHeading(), 0.0, 0.0);
    }
 
  public:
    inline const Vector3<kt_double>& GetPosition() const
    {
      return m_Position;
    }
 
    inline void SetPosition(const Vector3<kt_double>& rPosition)
    {
      m_Position = rPosition;
    }
 
    inline const Quaternion& GetOrientation() const
    {
      return m_Orientation;
    }
 
    inline void SetOrientation(const Quaternion& rOrientation)
    {
      m_Orientation = rOrientation;
    }
 
    inline std::string ToString()
    {
      std::stringstream converter;
      converter.precision(std::numeric_limits<double>::digits10);
 
      converter << GetPosition() << " " << GetOrientation();
 
      return converter.str();
    }
 
  public:
    inline Pose3& operator = (const Pose3& rOther)
    {
      m_Position = rOther.m_Position;
      m_Orientation = rOther.m_Orientation;
 
      return *this;
    }
 
    inline kt_bool operator == (const Pose3& rOther) const
    {
      return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation);
    }
 
    inline kt_bool operator != (const Pose3& rOther) const
    {
      return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation);
    }
 
    friend inline std::ostream& operator << (std::ostream& rStream, const Pose3& rPose)
    {
      rStream << rPose.GetPosition() << ", " << rPose.GetOrientation();
      return rStream;
    }
 
    friend inline std::istream& operator >> (std::istream& rStream, const Pose3& /*rPose*/)
    {
      // Implement me!!
      return rStream;
    }
 
  private:
    Vector3<kt_double> m_Position;
    Quaternion m_Orientation;
  };  // Pose3
 
 
  class Matrix3
  {
  public:
    Matrix3()
    {
      Clear();
    }
 
    inline Matrix3(const Matrix3& rOther)
    {
      memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
    }
 
  public:
    void SetToIdentity()
    {
      memset(m_Matrix, 0, 9*sizeof(kt_double));
 
      for (kt_int32s i = 0; i < 3; i++)
      {
        m_Matrix[i][i] = 1.0;
      }
    }
 
    void Clear()
    {
      memset(m_Matrix, 0, 9*sizeof(kt_double));
    }
 
    void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
    {
      kt_double cosRadians = cos(radians);
      kt_double sinRadians = sin(radians);
      kt_double oneMinusCos = 1.0 - cosRadians;
 
      kt_double xx = x * x;
      kt_double yy = y * y;
      kt_double zz = z * z;
 
      kt_double xyMCos = x * y * oneMinusCos;
      kt_double xzMCos = x * z * oneMinusCos;
      kt_double yzMCos = y * z * oneMinusCos;
 
      kt_double xSin = x * sinRadians;
      kt_double ySin = y * sinRadians;
      kt_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;
    }
 
    Matrix3 Transpose() const
    {
      Matrix3 transpose;
 
      for (kt_int32u row = 0; row < 3; row++)
      {
        for (kt_int32u col = 0; col < 3; col++)
        {
          transpose.m_Matrix[row][col] = m_Matrix[col][row];
        }
      }
 
      return transpose;
    }
 
    Matrix3 Inverse() const
    {
      Matrix3 kInverse = *this;
      kt_bool haveInverse = InverseFast(kInverse, 1e-14);
      if (haveInverse == false)
      {
        assert(false);
      }
      return kInverse;
    }
 
    kt_bool InverseFast(Matrix3& rkInverse, kt_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];
 
      kt_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;
      }
 
      kt_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;
    }
 
    inline std::string ToString() const
    {
      std::stringstream converter;
      converter.precision(std::numeric_limits<double>::digits10);
 
      for (int row = 0; row < 3; row++)
      {
        for (int col = 0; col < 3; col++)
        {
          converter << m_Matrix[row][col] << " ";
        }
      }
 
      return converter.str();
    }
 
  public:
    inline Matrix3& operator = (const Matrix3& rOther)
    {
      memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
      return *this;
    }
 
    inline kt_double& operator()(kt_int32u row, kt_int32u column)
    {
      return m_Matrix[row][column];
    }
 
    inline kt_double operator()(kt_int32u row, kt_int32u column) const
    {
      return m_Matrix[row][column];
    }
 
    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;
    }
 
    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;
    }
 
    inline void operator += (const Matrix3& rkMatrix)
    {
      for (kt_int32u row = 0; row < 3; row++)
      {
        for (kt_int32u col = 0; col < 3; col++)
        {
          m_Matrix[row][col] += rkMatrix.m_Matrix[row][col];
        }
      }
    }
 
    friend inline std::ostream& operator << (std::ostream& rStream, const Matrix3& rMatrix)
    {
      rStream << rMatrix.ToString();
      return rStream;
    }
 
  private:
    kt_double m_Matrix[3][3];
  };  // Matrix3
 
 
  class Matrix
  {
  public:
    Matrix(kt_int32u rows, kt_int32u columns)
      : m_Rows(rows)
      , m_Columns(columns)
      , m_pData(NULL)
    {
      Allocate();
 
      Clear();
    }
 
    virtual ~Matrix()
    {
      delete [] m_pData;
    }
 
  public:
    void Clear()
    {
      if (m_pData != NULL)
      {
        memset(m_pData, 0, sizeof(kt_double) * m_Rows * m_Columns);
      }
    }
 
    inline kt_int32u GetRows() const
    {
      return m_Rows;
    }
 
    inline kt_int32u GetColumns() const
    {
      return m_Columns;
    }
 
    inline kt_double& operator()(kt_int32u row, kt_int32u column)
    {
      RangeCheck(row, column);
 
      return m_pData[row + column * m_Rows];
    }
 
    inline const kt_double& operator()(kt_int32u row, kt_int32u column) const
    {
      RangeCheck(row, column);
 
      return m_pData[row + column * m_Rows];
    }
 
  private:
    void Allocate()
    {
      try
      {
        if (m_pData != NULL)
        {
          delete[] m_pData;
        }
 
        m_pData = new kt_double[m_Rows * m_Columns];
      }
      catch (const std::bad_alloc& ex)
      {
        throw Exception("Matrix allocation error");
      }
 
      if (m_pData == NULL)
      {
        throw Exception("Matrix allocation error");
      }
    }
 
    inline void RangeCheck(kt_int32u row, kt_int32u column) const
    {
      if (math::IsUpTo(row, m_Rows) == false)
      {
        throw Exception("Matrix - RangeCheck ERROR!!!!");
      }
 
      if (math::IsUpTo(column, m_Columns) == false)
      {
        throw Exception("Matrix - RangeCheck ERROR!!!!");
      }
    }
 
  private:
    kt_int32u m_Rows;
    kt_int32u m_Columns;
 
    kt_double* m_pData;
  };  // Matrix
 
 
  class BoundingBox2
  {
  public:
    /*
     * Default constructor
     */
    BoundingBox2()
      : m_Minimum(999999999999999999.99999, 999999999999999999.99999)
      , m_Maximum(-999999999999999999.99999, -999999999999999999.99999)
    {
    }
 
  public:
    inline const Vector2<kt_double>& GetMinimum() const
    {
      return m_Minimum;
    }
 
    inline void SetMinimum(const Vector2<kt_double>& mMinimum)
    {
      m_Minimum = mMinimum;
    }
 
    inline const Vector2<kt_double>& GetMaximum() const
    {
      return m_Maximum;
    }
 
    inline void SetMaximum(const Vector2<kt_double>& rMaximum)
    {
      m_Maximum = rMaximum;
    }
 
    inline Size2<kt_double> GetSize() const
    {
      Vector2<kt_double> size = m_Maximum - m_Minimum;
 
      return Size2<kt_double>(size.GetX(), size.GetY());
    }
 
    inline void Add(const Vector2<kt_double>& rPoint)
    {
      m_Minimum.MakeFloor(rPoint);
      m_Maximum.MakeCeil(rPoint);
    }
 
    inline void Add(const BoundingBox2& rBoundingBox)
    {
      Add(rBoundingBox.GetMinimum());
      Add(rBoundingBox.GetMaximum());
    }
 
    inline kt_bool IsInBounds(const Vector2<kt_double>& rPoint) const
    {
      return (math::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) &&
              math::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY()));
    }
 
  private:
    Vector2<kt_double> m_Minimum;
    Vector2<kt_double> m_Maximum;
  };  // BoundingBox2
 
 
  class Transform
  {
  public:
    Transform(const Pose2& rPose)
    {
      SetTransform(Pose2(), rPose);
    }
 
    Transform(const Pose2& rPose1, const Pose2& rPose2)
    {
      SetTransform(rPose1, rPose2);
    }
 
  public:
    inline Pose2 TransformPose(const Pose2& rSourcePose)
    {
      Pose2 newPosition = m_Transform + m_Rotation * rSourcePose;
      kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() + m_Transform.GetHeading());
 
      return Pose2(newPosition.GetPosition(), angle);
    }
 
    inline Pose2 InverseTransformPose(const Pose2& rSourcePose)
    {
      Pose2 newPosition = m_InverseRotation * (rSourcePose - m_Transform);
      kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() - m_Transform.GetHeading());
 
      // components of transform
      return Pose2(newPosition.GetPosition(), angle);
    }
 
  private:
    void SetTransform(const Pose2& rPose1, const Pose2& rPose2)
    {
      if (rPose1 == rPose2)
      {
        m_Rotation.SetToIdentity();
        m_InverseRotation.SetToIdentity();
        m_Transform = Pose2();
        return;
      }
 
      // heading transformation
      m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading());
      m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading());
 
      // position transformation
      Pose2 newPosition;
      if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0)
      {
        newPosition = rPose2 - m_Rotation * rPose1;
      }
      else
      {
        newPosition = rPose2;
      }
 
      m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading());
    }
 
  private:
    // pose transformation
    Pose2 m_Transform;
 
    Matrix3 m_Rotation;
    Matrix3 m_InverseRotation;
  };  // Transform
 
 
  typedef enum
  {
    LaserRangeFinder_Custom = 0,
 
    LaserRangeFinder_Sick_LMS100 = 1,
    LaserRangeFinder_Sick_LMS200 = 2,
    LaserRangeFinder_Sick_LMS291 = 3,
 
    LaserRangeFinder_Hokuyo_UTM_30LX = 4,
    LaserRangeFinder_Hokuyo_URG_04LX = 5
  } LaserRangeFinderType;
 
 
  class AbstractParameter
  {
  public:
    AbstractParameter(const std::string& rName, ParameterManager* pParameterManger = NULL)
      : m_Name(rName)
    {
      // if parameter manager is provided add myself to it!
      if (pParameterManger != NULL)
      {
        pParameterManger->Add(this);
      }
    }
 
    AbstractParameter(const std::string& rName,
                      const std::string& rDescription,
                      ParameterManager* pParameterManger = NULL)
      : m_Name(rName)
      , m_Description(rDescription)
    {
      // if parameter manager is provided add myself to it!
      if (pParameterManger != NULL)
      {
        pParameterManger->Add(this);
      }
    }
 
    virtual ~AbstractParameter()
    {
    }
 
  public:
    inline const std::string& GetName() const
    {
      return m_Name;
    }
 
    inline const std::string& GetDescription() const
    {
      return m_Description;
    }
 
    virtual const std::string GetValueAsString() const = 0;
 
    virtual void SetValueFromString(const std::string& rStringValue) = 0;
 
    virtual AbstractParameter* Clone() = 0;
 
  public:
    friend std::ostream& operator << (std::ostream& rStream, const AbstractParameter& rParameter)
    {
      rStream.precision(6);
      rStream.flags(std::ios::fixed);
 
      rStream << rParameter.GetName() << " = " << rParameter.GetValueAsString();
      return rStream;
    }
 
  private:
    std::string m_Name;
    std::string m_Description;
  };  // AbstractParameter
 
 
  template<typename T>
  class Parameter : public AbstractParameter
  {
  public:
    Parameter(const std::string& rName, T value, ParameterManager* pParameterManger = NULL)
      : AbstractParameter(rName, pParameterManger)
      , m_Value(value)
    {
    }
 
    Parameter(const std::string& rName,
              const std::string& rDescription,
              T value,
              ParameterManager* pParameterManger = NULL)
      : AbstractParameter(rName, rDescription, pParameterManger)
      , m_Value(value)
    {
    }
 
    virtual ~Parameter()
    {
    }
 
  public:
    inline const T& GetValue() const
    {
      return m_Value;
    }
 
    inline void SetValue(const T& rValue)
    {
      m_Value = rValue;
    }
 
    virtual const std::string GetValueAsString() const
    {
      std::stringstream converter;
      converter << m_Value;
      return converter.str();
    }
 
    virtual void SetValueFromString(const std::string& rStringValue)
    {
      std::stringstream converter;
      converter.str(rStringValue);
      converter >> m_Value;
    }
 
    virtual Parameter* Clone()
    {
      return new Parameter(GetName(), GetDescription(), GetValue());
    }
 
  public:
    Parameter& operator = (const Parameter& rOther)
    {
      m_Value = rOther.m_Value;
 
      return *this;
    }
 
    T operator = (T value)
    {
      m_Value = value;
 
      return m_Value;
    }
 
  protected:
    T m_Value;
  };  // Parameter
 
  template<>
  inline void Parameter<kt_double>::SetValueFromString(const std::string& rStringValue)
  {
    int precision = std::numeric_limits<double>::digits10;
    std::stringstream converter;
    converter.precision(precision);
 
    converter.str(rStringValue);
 
    m_Value = 0.0;
    converter >> m_Value;
  }
 
  template<>
  inline const std::string Parameter<kt_double>::GetValueAsString() const
  {
    std::stringstream converter;
    converter.precision(std::numeric_limits<double>::digits10);
    converter << m_Value;
    return converter.str();
  }
 
  template<>
  inline void Parameter<kt_bool>::SetValueFromString(const std::string& rStringValue)
  {
    if (rStringValue == "true" || rStringValue == "TRUE")
    {
      m_Value = true;
    }
    else
    {
      m_Value = false;
    }
  }
 
  template<>
  inline const std::string Parameter<kt_bool>::GetValueAsString() const
  {
    if (m_Value == true)
    {
      return "true";
    }
 
    return "false";
  }
 
 
  class ParameterEnum : public Parameter<kt_int32s>
  {
    typedef std::map<std::string, kt_int32s> EnumMap;
 
  public:
    ParameterEnum(const std::string& rName, kt_int32s value, ParameterManager* pParameterManger = NULL)
      : Parameter<kt_int32s>(rName, value, pParameterManger)
    {
    }
 
    virtual ~ParameterEnum()
    {
    }
 
  public:
    virtual Parameter<kt_int32s>* Clone()
    {
      ParameterEnum* pEnum = new ParameterEnum(GetName(), GetValue());
 
      pEnum->m_EnumDefines = m_EnumDefines;
 
      return pEnum;
    }
 
    virtual void SetValueFromString(const std::string& rStringValue)
    {
      if (m_EnumDefines.find(rStringValue) != m_EnumDefines.end())
      {
        m_Value = m_EnumDefines[rStringValue];
      }
      else
      {
        std::string validValues;
 
        const_forEach(EnumMap, &m_EnumDefines)
        {
          validValues += iter->first + ", ";
        }
 
        throw Exception("Unable to set enum: " + rStringValue + ". Valid values are: " + validValues);
      }
    }
 
    virtual const std::string GetValueAsString() const
    {
      const_forEach(EnumMap, &m_EnumDefines)
      {
        if (iter->second == m_Value)
        {
          return iter->first;
        }
      }
 
      throw Exception("Unable to lookup enum");
    }
 
    void DefineEnumValue(kt_int32s value, const std::string& rName)
    {
      if (m_EnumDefines.find(rName) == m_EnumDefines.end())
      {
        m_EnumDefines[rName] = value;
      }
      else
      {
        std::cerr << "Overriding enum value: " << m_EnumDefines[rName] << " with " << value << std::endl;
 
        m_EnumDefines[rName] = value;
 
        assert(false);
      }
    }
 
  public:
    ParameterEnum& operator = (const ParameterEnum& rOther)
    {
      SetValue(rOther.GetValue());
 
      return *this;
    }
 
    kt_int32s operator = (kt_int32s value)
    {
      SetValue(value);
 
      return m_Value;
    }
 
  private:
    EnumMap m_EnumDefines;
  };  // ParameterEnum
 
 
  class Parameters : public Object
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(Parameters)
    // @endcond
 
  public:
    Parameters(const std::string& rName)
      : Object(rName)
    {
    }
 
    virtual ~Parameters()
    {
    }
 
  private:
    Parameters(const Parameters&);
    const Parameters& operator=(const Parameters&);
  };  // Parameters
 
 
  class SensorData;
 
  class KARTO_EXPORT Sensor : public Object
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(Sensor)
    // @endcond
 
  protected:
    Sensor(const Name& rName);
 
  public:
    virtual ~Sensor();
 
  public:
    inline const Pose2& GetOffsetPose() const
    {
      return m_pOffsetPose->GetValue();
    }
 
    inline void SetOffsetPose(const Pose2& rPose)
    {
      m_pOffsetPose->SetValue(rPose);
    }
 
    virtual kt_bool Validate() = 0;
 
    virtual kt_bool Validate(SensorData* pSensorData) = 0;
 
  private:
    Sensor(const Sensor&);
 
    const Sensor& operator=(const Sensor&);
 
  private:
    Parameter<Pose2>* m_pOffsetPose;
  };  // Sensor
 
  typedef std::vector<Sensor*> SensorVector;
 
 
  typedef std::map<Name, Sensor*> SensorManagerMap;
 
  class KARTO_EXPORT SensorManager
  {
  public:
    SensorManager()
    {
    }
 
    virtual ~SensorManager()
    {
    }
 
  public:
    static SensorManager* GetInstance();
 
  public:
    void RegisterSensor(Sensor* pSensor, kt_bool override = false)
    {
      Validate(pSensor);
 
      if ((m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) && !override)
      {
        throw Exception("Cannot register sensor: already registered: [" +
                        pSensor->GetName().ToString() +
                        "] (Consider setting 'override' to true)");
      }
 
      std::cout << "Registering sensor: [" << pSensor->GetName().ToString() << "]" << std::endl;
 
      m_Sensors[pSensor->GetName()] = pSensor;
    }
 
    void UnregisterSensor(Sensor* pSensor)
    {
      Validate(pSensor);
 
      if (m_Sensors.find(pSensor->GetName()) != m_Sensors.end())
      {
        std::cout << "Unregistering sensor: " << pSensor->GetName().ToString() << std::endl;
 
        m_Sensors.erase(pSensor->GetName());
      }
      else
      {
        throw Exception("Cannot unregister sensor: not registered: [" + pSensor->GetName().ToString() + "]");
      }
    }
 
    Sensor* GetSensorByName(const Name& rName)
    {
      if (m_Sensors.find(rName) != m_Sensors.end())
      {
        return m_Sensors[rName];
      }
 
      throw Exception("Sensor not registered: [" + rName.ToString() + "] (Did you add the sensor to the Dataset?)");
    }
 
    template<class T>
    T* GetSensorByName(const Name& rName)
    {
      Sensor* pSensor = GetSensorByName(rName);
 
      return dynamic_cast<T*>(pSensor);
    }
 
    SensorVector GetAllSensors()
    {
      SensorVector sensors;
 
      forEach(SensorManagerMap, &m_Sensors)
      {
        sensors.push_back(iter->second);
      }
 
      return sensors;
    }
 
  protected:
    static void Validate(Sensor* pSensor)
    {
      if (pSensor == NULL)
      {
        throw Exception("Invalid sensor:  NULL");
      }
      else if (pSensor->GetName().ToString() == "")
      {
        throw Exception("Invalid sensor:  nameless");
      }
    }
 
  protected:
    SensorManagerMap m_Sensors;
  };
 
 
  class Drive : public Sensor
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(Drive)
    // @endcond
 
  public:
    Drive(const std::string& rName)
      : Sensor(rName)
    {
    }
 
    virtual ~Drive()
    {
    }
 
  public:
    virtual kt_bool Validate()
    {
      return true;
    }
 
    virtual kt_bool Validate(SensorData* pSensorData)
    {
      if (pSensorData == NULL)
      {
        return false;
      }
 
      return true;
    }
 
  private:
    Drive(const Drive&);
    const Drive& operator=(const Drive&);
  };  // class Drive
 
 
  class LocalizedRangeScan;
  class CoordinateConverter;
 
  class KARTO_EXPORT LaserRangeFinder : public Sensor
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(LaserRangeFinder)
    // @endcond
 
  public:
    virtual ~LaserRangeFinder()
    {
    }
 
  public:
    inline kt_double GetMinimumRange() const
    {
      return m_pMinimumRange->GetValue();
    }
 
    inline void SetMinimumRange(kt_double minimumRange)
    {
      m_pMinimumRange->SetValue(minimumRange);
 
      SetRangeThreshold(GetRangeThreshold());
    }
 
    inline kt_double GetMaximumRange() const
    {
      return m_pMaximumRange->GetValue();
    }
 
    inline void SetMaximumRange(kt_double maximumRange)
    {
      m_pMaximumRange->SetValue(maximumRange);
 
      SetRangeThreshold(GetRangeThreshold());
    }
 
    inline kt_double GetRangeThreshold() const
    {
      return m_pRangeThreshold->GetValue();
    }
 
    inline void SetRangeThreshold(kt_double rangeThreshold)
    {
      // make sure rangeThreshold is within laser range finder range
      m_pRangeThreshold->SetValue(math::Clip(rangeThreshold, GetMinimumRange(), GetMaximumRange()));
 
      if (math::DoubleEqual(GetRangeThreshold(), rangeThreshold) == false)
      {
        std::cout << "Info: clipped range threshold to be within minimum and maximum range!" << std::endl;
      }
    }
 
    inline kt_double GetMinimumAngle() const
    {
      return m_pMinimumAngle->GetValue();
    }
 
    inline void SetMinimumAngle(kt_double minimumAngle)
    {
      m_pMinimumAngle->SetValue(minimumAngle);
 
      Update();
    }
 
    inline kt_double GetMaximumAngle() const
    {
      return m_pMaximumAngle->GetValue();
    }
 
    inline void SetMaximumAngle(kt_double maximumAngle)
    {
      m_pMaximumAngle->SetValue(maximumAngle);
 
      Update();
    }
 
    inline kt_double GetAngularResolution() const
    {
      return m_pAngularResolution->GetValue();
    }
 
    inline void SetAngularResolution(kt_double angularResolution)
    {
      if (m_pType->GetValue() == LaserRangeFinder_Custom)
      {
        m_pAngularResolution->SetValue(angularResolution);
      }
      else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS100)
      {
        if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
        {
          m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
        }
        else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
        {
          m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
        }
        else
        {
          std::stringstream stream;
          stream << "Invalid resolution for Sick LMS100:  ";
          stream << angularResolution;
          throw Exception(stream.str());
        }
      }
      else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS200 ||
               m_pType->GetValue() == LaserRangeFinder_Sick_LMS291)
      {
        if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
        {
          m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
        }
        else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
        {
          m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
        }
        else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(1.00)))
        {
          m_pAngularResolution->SetValue(math::DegreesToRadians(1.00));
        }
        else
        {
          std::stringstream stream;
          stream << "Invalid resolution for Sick LMS291:  ";
          stream << angularResolution;
          throw Exception(stream.str());
        }
      }
      else if (m_pType->GetValue() == LaserRangeFinder_Hokuyo_UTM_30LX)
      {
        if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
        {
          m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
        }
        else
        {
          std::stringstream stream;
          stream << "Invalid resolution for Hokuyo_UTM_30LX:  ";
          stream << angularResolution;
          throw Exception(stream.str());
        }
      }
      else if (m_pType->GetValue() == LaserRangeFinder_Hokuyo_URG_04LX)
      {
        if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.352)))
        {
          m_pAngularResolution->SetValue(math::DegreesToRadians(0.352));
        }
        else
        {
          std::stringstream stream;
          stream << "Invalid resolution for Hokuyo_URG_04LX:  ";
          stream << angularResolution;
          throw Exception(stream.str());
        }
      }
      else
      {
        throw Exception("Can't set angular resolution, please create a LaserRangeFinder of type Custom");
      }
 
      Update();
    }
 
    inline kt_int32s GetType()
    {
      return m_pType->GetValue();
    }
 
    inline kt_int32u GetNumberOfRangeReadings() const
    {
      return m_NumberOfRangeReadings;
    }
 
    virtual kt_bool Validate()
    {
      Update();
 
      if (math::InRange(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange()) == false)
      {
        std::cout << "Please set range threshold to a value between ["
                  << GetMinimumRange() << ";" << GetMaximumRange() << "]" << std::endl;
        return false;
      }
 
      return true;
    }
 
    virtual kt_bool Validate(SensorData* pSensorData);
 
    const PointVectorDouble GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan,
                                            CoordinateConverter* pCoordinateConverter,
                                            kt_bool ignoreThresholdPoints = true,
                                            kt_bool flipY = false) const;
 
  public:
    static LaserRangeFinder* CreateLaserRangeFinder(LaserRangeFinderType type, const Name& rName)
    {
      LaserRangeFinder* pLrf = NULL;
 
      switch (type)
      {
        // see http://www.hizook.com/files/publications/SICK_LMS100.pdf
        // set range threshold to 18m
        case LaserRangeFinder_Sick_LMS100:
        {
          pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 100"));
 
          // Sensing range is 18 meters (at 10% reflectivity, max range of 20 meters), with an error of about 20mm
          pLrf->m_pMinimumRange->SetValue(0.0);
          pLrf->m_pMaximumRange->SetValue(20.0);
 
          // 270 degree range, 50 Hz
          pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135));
          pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135));
 
          // 0.25 degree angular resolution
          pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
 
          pLrf->m_NumberOfRangeReadings = 1081;
        }
        break;
 
        // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
        // set range threshold to 10m
        case LaserRangeFinder_Sick_LMS200:
        {
          pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 200"));
 
          // Sensing range is 80 meters
          pLrf->m_pMinimumRange->SetValue(0.0);
          pLrf->m_pMaximumRange->SetValue(80.0);
 
          // 180 degree range, 75 Hz
          pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
          pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
 
          // 0.5 degree angular resolution
          pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5));
 
          pLrf->m_NumberOfRangeReadings = 361;
        }
        break;
 
        // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
        // set range threshold to 30m
        case LaserRangeFinder_Sick_LMS291:
        {
          pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 291"));
 
          // Sensing range is 80 meters
          pLrf->m_pMinimumRange->SetValue(0.0);
          pLrf->m_pMaximumRange->SetValue(80.0);
 
          // 180 degree range, 75 Hz
          pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
          pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
 
          // 0.5 degree angular resolution
          pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5));
 
          pLrf->m_NumberOfRangeReadings = 361;
        }
        break;
 
        // see http://www.hizook.com/files/publications/Hokuyo_UTM_LaserRangeFinder_LIDAR.pdf
        // set range threshold to 30m
        case LaserRangeFinder_Hokuyo_UTM_30LX:
        {
          pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo UTM-30LX"));
 
          // Sensing range is 30 meters
          pLrf->m_pMinimumRange->SetValue(0.1);
          pLrf->m_pMaximumRange->SetValue(30.0);
 
          // 270 degree range, 40 Hz
          pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135));
          pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135));
 
          // 0.25 degree angular resolution
          pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
 
          pLrf->m_NumberOfRangeReadings = 1081;
        }
        break;
 
        // see http://www.hizook.com/files/publications/HokuyoURG_Datasheet.pdf
        // set range threshold to 3.5m
        case LaserRangeFinder_Hokuyo_URG_04LX:
        {
          pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo URG-04LX"));
 
          // Sensing range is 4 meters. It has detection problems when
          // scanning absorptive surfaces (such as black trimming).
          pLrf->m_pMinimumRange->SetValue(0.02);
          pLrf->m_pMaximumRange->SetValue(4.0);
 
          // 240 degree range, 10 Hz
          pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-120));
          pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(120));
 
          // 0.352 degree angular resolution
          pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.352));
 
          pLrf->m_NumberOfRangeReadings = 751;
        }
        break;
 
        case LaserRangeFinder_Custom:
        {
          pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("User-Defined LaserRangeFinder"));
 
          // Sensing range is 80 meters.
          pLrf->m_pMinimumRange->SetValue(0.0);
          pLrf->m_pMaximumRange->SetValue(80.0);
 
          // 180 degree range
          pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
          pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
 
          // 1.0 degree angular resolution
          pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(1.0));
 
          pLrf->m_NumberOfRangeReadings = 181;
        }
        break;
      }
 
      if (pLrf != NULL)
      {
        pLrf->m_pType->SetValue(type);
 
        Pose2 defaultOffset;
        pLrf->SetOffsetPose(defaultOffset);
      }
 
      return pLrf;
    }
 
  private:
    LaserRangeFinder(const Name& rName)
      : Sensor(rName)
      , m_NumberOfRangeReadings(0)
    {
      m_pMinimumRange = new Parameter<kt_double>("MinimumRange", 0.0, GetParameterManager());
      m_pMaximumRange = new Parameter<kt_double>("MaximumRange", 80.0, GetParameterManager());
 
      m_pMinimumAngle = new Parameter<kt_double>("MinimumAngle", -KT_PI_2, GetParameterManager());
      m_pMaximumAngle = new Parameter<kt_double>("MaximumAngle", KT_PI_2, GetParameterManager());
 
      m_pAngularResolution = new Parameter<kt_double>("AngularResolution",
                                                      math::DegreesToRadians(1),
                                                      GetParameterManager());
 
      m_pRangeThreshold = new Parameter<kt_double>("RangeThreshold", 12.0, GetParameterManager());
 
      m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager());
      m_pType->DefineEnumValue(LaserRangeFinder_Custom, "Custom");
      m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS100, "Sick_LMS100");
      m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS200, "Sick_LMS200");
      m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS291, "Sick_LMS291");
      m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_UTM_30LX, "Hokuyo_UTM_30LX");
      m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_URG_04LX, "Hokuyo_URG_04LX");
    }
 
    void Update()
    {
      m_NumberOfRangeReadings = static_cast<kt_int32u>(math::Round((GetMaximumAngle() -
                                                                    GetMinimumAngle())
                                                                    / GetAngularResolution()) + 1);
    }
 
  private:
    LaserRangeFinder(const LaserRangeFinder&);
    const LaserRangeFinder& operator=(const LaserRangeFinder&);
 
  private:
    // sensor m_Parameters
    Parameter<kt_double>* m_pMinimumAngle;
    Parameter<kt_double>* m_pMaximumAngle;
 
    Parameter<kt_double>* m_pAngularResolution;
 
    Parameter<kt_double>* m_pMinimumRange;
    Parameter<kt_double>* m_pMaximumRange;
 
    Parameter<kt_double>* m_pRangeThreshold;
 
    ParameterEnum* m_pType;
 
    kt_int32u m_NumberOfRangeReadings;
 
    // static std::string LaserRangeFinderTypeNames[6];
  };  // LaserRangeFinder
 
 
  typedef enum
  {
    GridStates_Unknown = 0,
    GridStates_Occupied = 100,
    GridStates_Free = 255
  } GridStates;
 
 
  class CoordinateConverter
  {
  public:
    CoordinateConverter()
      : m_Scale(20.0)
    {
    }
 
  public:
    inline kt_double Transform(kt_double value)
    {
      return value * m_Scale;
    }
 
    inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double>& rWorld, kt_bool flipY = false) const
    {
      kt_double gridX = (rWorld.GetX() - m_Offset.GetX()) * m_Scale;
      kt_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<kt_int32s>(static_cast<kt_int32s>(math::Round(gridX)), static_cast<kt_int32s>(math::Round(gridY)));
    }
 
    inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s>& rGrid, kt_bool flipY = false) const
    {
      kt_double worldX = m_Offset.GetX() + rGrid.GetX() / m_Scale;
      kt_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<kt_double>(worldX, worldY);
    }
 
    inline kt_double GetScale() const
    {
      return m_Scale;
    }
 
    inline void SetScale(kt_double scale)
    {
      m_Scale = scale;
    }
 
    inline const Vector2<kt_double>& GetOffset() const
    {
      return m_Offset;
    }
 
    inline void SetOffset(const Vector2<kt_double>& rOffset)
    {
      m_Offset = rOffset;
    }
 
    inline void SetSize(const Size2<kt_int32s>& rSize)
    {
      m_Size = rSize;
    }
 
    inline const Size2<kt_int32s>& GetSize() const
    {
      return m_Size;
    }
 
    inline kt_double GetResolution() const
    {
      return 1.0 / m_Scale;
    }
 
    inline void SetResolution(kt_double resolution)
    {
      m_Scale = 1.0 / resolution;
    }
 
    inline BoundingBox2 GetBoundingBox() const
    {
      BoundingBox2 box;
 
      kt_double minX = GetOffset().GetX();
      kt_double minY = GetOffset().GetY();
      kt_double maxX = minX + GetSize().GetWidth() * GetResolution();
      kt_double maxY = minY + GetSize().GetHeight() * GetResolution();
 
      box.SetMinimum(GetOffset());
      box.SetMaximum(Vector2<kt_double>(maxX, maxY));
      return box;
    }
 
  private:
    Size2<kt_int32s> m_Size;
    kt_double m_Scale;
 
    Vector2<kt_double> m_Offset;
  };  // CoordinateConverter
 
 
  template<typename T>
  class Grid
  {
  public:
    static Grid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
    {
      Grid* pGrid = new Grid(width, height);
 
      pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution);
 
      return pGrid;
    }
 
    virtual ~Grid()
    {
      delete [] m_pData;
      delete m_pCoordinateConverter;
    }
 
  public:
    void Clear()
    {
      memset(m_pData, 0, GetDataSize() * sizeof(T));
    }
 
    Grid* Clone()
    {
      Grid* pGrid = CreateGrid(GetWidth(), GetHeight(), GetResolution());
      pGrid->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
 
      memcpy(pGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
 
      return pGrid;
    }
 
    virtual void Resize(kt_int32s width, kt_int32s height)
    {
      m_Width = width;
      m_Height = height;
      m_WidthStep = math::AlignValue<kt_int32s>(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<kt_int32s>(width, height));
      }
      catch(...)
      {
        m_pData = NULL;
 
        m_Width = 0;
        m_Height = 0;
        m_WidthStep = 0;
      }
 
      Clear();
    }
 
    inline kt_bool IsValidGridIndex(const Vector2<kt_int32s>& rGrid) const
    {
      return (math::IsUpTo(rGrid.GetX(), m_Width) && math::IsUpTo(rGrid.GetY(), m_Height));
    }
 
    virtual kt_int32s GridIndex(const Vector2<kt_int32s>& rGrid, kt_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 << ")";
          throw Exception(error.str());
        }
      }
 
      kt_int32s index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep);
 
      if (boundaryCheck == true)
      {
        assert(math::IsUpTo(index, GetDataSize()));
      }
 
      return index;
    }
 
    Vector2<kt_int32s> IndexToGrid(kt_int32s index) const
    {
      Vector2<kt_int32s> grid;
 
      grid.SetY(index / m_WidthStep);
      grid.SetX(index - grid.GetY() * m_WidthStep);
 
      return grid;
    }
 
    inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double>& rWorld, kt_bool flipY = false) const
    {
      return GetCoordinateConverter()->WorldToGrid(rWorld, flipY);
    }
 
    inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s>& rGrid, kt_bool flipY = false) const
    {
      return GetCoordinateConverter()->GridToWorld(rGrid, flipY);
    }
 
    T* GetDataPointer(const Vector2<kt_int32s>& rGrid)
    {
      kt_int32s index = GridIndex(rGrid, true);
      return m_pData + index;
    }
 
    T* GetDataPointer(const Vector2<kt_int32s>& rGrid) const
    {
      kt_int32s index = GridIndex(rGrid, true);
      return m_pData + index;
    }
 
    inline kt_int32s GetWidth() const
    {
      return m_Width;
    };
 
    inline kt_int32s GetHeight() const
    {
      return m_Height;
    };
 
    inline const Size2<kt_int32s> GetSize() const
    {
      return Size2<kt_int32s>(m_Width, m_Height);
    }
 
    inline kt_int32s GetWidthStep() const
    {
      return m_WidthStep;
    }
 
    inline T* GetDataPointer()
    {
      return m_pData;
    }
 
    inline T* GetDataPointer() const
    {
      return m_pData;
    }
 
    inline kt_int32s GetDataSize() const
    {
      return m_WidthStep * m_Height;
    }
 
    inline T GetValue(const Vector2<kt_int32s>& rGrid) const
    {
      kt_int32s index = GridIndex(rGrid);
      return m_pData[index];
    }
 
    inline CoordinateConverter* GetCoordinateConverter() const
    {
      return m_pCoordinateConverter;
    }
 
    inline kt_double GetResolution() const
    {
      return GetCoordinateConverter()->GetResolution();
    }
 
    inline BoundingBox2 GetBoundingBox() const
    {
      return GetCoordinateConverter()->GetBoundingBox();
    }
 
    void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor* f = NULL)
    {
      kt_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);
      }
 
      kt_int32s deltaX = x1 - x0;
      kt_int32s deltaY = abs(y1 - y0);
      kt_int32s error = 0;
      kt_int32s ystep;
      kt_int32s y = y0;
 
      if (y0 < y1)
      {
        ystep = 1;
      }
      else
      {
        ystep = -1;
      }
 
      kt_int32s pointX;
      kt_int32s pointY;
      for (kt_int32s 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<kt_int32s> gridIndex(pointX, pointY);
        if (IsValidGridIndex(gridIndex))
        {
          kt_int32s index = GridIndex(gridIndex, false);
          T* pGridPointer = GetDataPointer();
          pGridPointer[index]++;
 
          if (f != NULL)
          {
            (*f)(index);
          }
        }
      }
    }
 
  protected:
    Grid(kt_int32s width, kt_int32s height)
      : m_pData(NULL)
      , m_pCoordinateConverter(NULL)
    {
      Resize(width, height);
    }
 
  private:
    kt_int32s m_Width;       // width of grid
    kt_int32s m_Height;      // height of grid
    kt_int32s 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
 
 
  class CustomData : public Object
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(CustomData)
    // @endcond
 
  public:
    CustomData()
      : Object()
    {
    }
 
    virtual ~CustomData()
    {
    }
 
  public:
    virtual const std::string Write() const = 0;
 
    virtual void Read(const std::string& rValue) = 0;
 
  private:
    CustomData(const CustomData&);
    const CustomData& operator=(const CustomData&);
  };
 
  typedef std::vector<CustomData*> CustomDataVector;
 
 
  class KARTO_EXPORT SensorData : public Object
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(SensorData)
    // @endcond
 
  public:
    virtual ~SensorData();
 
  public:
    inline kt_int32s GetStateId() const
    {
      return m_StateId;
    }
 
    inline void SetStateId(kt_int32s stateId)
    {
      m_StateId = stateId;
    }
 
    inline kt_int32s GetUniqueId() const
    {
      return m_UniqueId;
    }
 
    inline void SetUniqueId(kt_int32u uniqueId)
    {
      m_UniqueId = uniqueId;
    }
 
    inline kt_double GetTime() const
    {
      return m_Time;
    }
 
    inline void SetTime(kt_double time)
    {
      m_Time = time;
    }
 
    inline const Name& GetSensorName() const
    {
      return m_SensorName;
    }
 
    inline void AddCustomData(CustomData* pCustomData)
    {
      m_CustomData.push_back(pCustomData);
    }
 
    inline const CustomDataVector& GetCustomData() const
    {
      return m_CustomData;
    }
 
  protected:
    SensorData(const Name& rSensorName);
 
  private:
    SensorData(const SensorData&);
 
    const SensorData& operator=(const SensorData&);
 
  private:
    kt_int32s m_StateId;
 
    kt_int32s m_UniqueId;
 
    Name m_SensorName;
 
    kt_double m_Time;
 
    CustomDataVector m_CustomData;
  };
 
 
  typedef std::vector<kt_double> RangeReadingsVector;
 
  class LaserRangeScan : public SensorData
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(LaserRangeScan)
    // @endcond
 
  public:
    LaserRangeScan(const Name& rSensorName)
      : SensorData(rSensorName)
      , m_pRangeReadings(NULL)
      , m_NumberOfRangeReadings(0)
    {
    }
 
    LaserRangeScan(const Name& rSensorName, const RangeReadingsVector& rRangeReadings)
      : SensorData(rSensorName)
      , m_pRangeReadings(NULL)
      , m_NumberOfRangeReadings(0)
    {
      assert(rSensorName.ToString() != "");
 
      SetRangeReadings(rRangeReadings);
    }
 
    virtual ~LaserRangeScan()
    {
      delete [] m_pRangeReadings;
    }
 
  public:
    inline kt_double* GetRangeReadings() const
    {
      return m_pRangeReadings;
    }
 
    inline RangeReadingsVector GetRangeReadingsVector() const
    {
      return RangeReadingsVector(m_pRangeReadings, m_pRangeReadings + m_NumberOfRangeReadings);
    }
 
    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<kt_int32u>(rRangeReadings.size());
 
          // allocate range readings
          m_pRangeReadings = new kt_double[m_NumberOfRangeReadings];
        }
 
        // copy readings
        kt_int32u index = 0;
        const_forEach(RangeReadingsVector, &rRangeReadings)
        {
          m_pRangeReadings[index++] = *iter;
        }
      }
      else
      {
        delete [] m_pRangeReadings;
        m_pRangeReadings = NULL;
      }
    }
 
    inline LaserRangeFinder* GetLaserRangeFinder() const
    {
      return SensorManager::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorName());
    }
 
    inline kt_int32u GetNumberOfRangeReadings() const
    {
      return m_NumberOfRangeReadings;
    }
 
  private:
    LaserRangeScan(const LaserRangeScan&);
    const LaserRangeScan& operator=(const LaserRangeScan&);
 
  private:
    kt_double* m_pRangeReadings;
    kt_int32u m_NumberOfRangeReadings;
  };  // LaserRangeScan
 
 
  class DrivePose : public SensorData
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(DrivePose)
    // @endcond
 
  public:
    DrivePose(const Name& rSensorName)
      : SensorData(rSensorName)
    {
    }
 
    virtual ~DrivePose()
    {
    }
 
  public:
    inline const Pose3& GetOdometricPose() const
    {
      return m_OdometricPose;
    }
 
    inline void SetOdometricPose(const Pose3& rPose)
    {
      m_OdometricPose = rPose;
    }
 
  private:
    DrivePose(const DrivePose&);
    const DrivePose& operator=(const DrivePose&);
 
  private:
    Pose3 m_OdometricPose;
  };  // class DrivePose
 
 
  class LocalizedRangeScan : public LaserRangeScan
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(LocalizedRangeScan)
    // @endcond
 
  public:
    LocalizedRangeScan(const Name& rSensorName, const RangeReadingsVector& rReadings)
      : LaserRangeScan(rSensorName, rReadings)
      , m_IsDirty(true)
    {
    }
 
    virtual ~LocalizedRangeScan()
    {
    }
 
  private:
    mutable boost::shared_mutex m_Lock;
 
  public:
    inline const Pose2& GetOdometricPose() const
    {
      return m_OdometricPose;
    }
 
    inline void SetOdometricPose(const Pose2& rPose)
    {
      m_OdometricPose = rPose;
    }
 
    inline const Pose2& GetCorrectedPose() const
    {
      return m_CorrectedPose;
    }
 
    inline void SetCorrectedPose(const Pose2& rPose)
    {
      m_CorrectedPose = rPose;
 
      m_IsDirty = true;
    }
 
    inline const Pose2& GetBarycenterPose() const
    {
      boost::shared_lock<boost::shared_mutex> lock(m_Lock);
      if (m_IsDirty)
      {
        // throw away constness and do an update!
        lock.unlock();
        boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
        const_cast<LocalizedRangeScan*>(this)->Update();
      }
 
      return m_BarycenterPose;
    }
 
    inline Pose2 GetReferencePose(kt_bool useBarycenter) const
    {
      boost::shared_lock<boost::shared_mutex> lock(m_Lock);
      if (m_IsDirty)
      {
        // throw away constness and do an update!
        lock.unlock();
        boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
        const_cast<LocalizedRangeScan*>(this)->Update();
      }
 
      return useBarycenter ? GetBarycenterPose() : GetSensorPose();
    }
 
    inline Pose2 GetSensorPose() const
    {
      return GetSensorAt(m_CorrectedPose);
    }
 
    void SetSensorPose(const Pose2& rScanPose)
    {
      Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose();
      kt_double offsetLength = deviceOffsetPose2.GetPosition().Length();
      kt_double offsetHeading = deviceOffsetPose2.GetHeading();
      kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX());
      kt_double correctedHeading = math::NormalizeAngle(rScanPose.GetHeading());
      Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading),
                                      offsetLength * sin(correctedHeading + angleoffset - offsetHeading),
                                      offsetHeading);
 
      m_CorrectedPose = rScanPose - worldSensorOffset;
 
      Update();
    }
 
    inline Pose2 GetSensorAt(const Pose2& rPose) const
    {
      return Transform(rPose).TransformPose(GetLaserRangeFinder()->GetOffsetPose());
    }
 
    inline const BoundingBox2& GetBoundingBox() const
    {
      boost::shared_lock<boost::shared_mutex> lock(m_Lock);
      if (m_IsDirty)
      {
        // throw away constness and do an update!
        lock.unlock();
        boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
        const_cast<LocalizedRangeScan*>(this)->Update();
      }
 
      return m_BoundingBox;
    }
 
    inline const PointVectorDouble& GetPointReadings(kt_bool wantFiltered = false) const
    {
      boost::shared_lock<boost::shared_mutex> lock(m_Lock);
      if (m_IsDirty)
      {
        // throw away constness and do an update!
        lock.unlock();
        boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
        const_cast<LocalizedRangeScan*>(this)->Update();
      }
 
      if (wantFiltered == true)
      {
        return m_PointReadings;
      }
      else
      {
        return m_UnfilteredPointReadings;
      }
    }
 
  private:
    virtual void Update()
    {
      LaserRangeFinder* pLaserRangeFinder = GetLaserRangeFinder();
 
      if (pLaserRangeFinder != NULL)
      {
        m_PointReadings.clear();
        m_UnfilteredPointReadings.clear();
 
        kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold();
        kt_double minimumAngle = pLaserRangeFinder->GetMinimumAngle();
        kt_double angularResolution = pLaserRangeFinder->GetAngularResolution();
        Pose2 scanPose = GetSensorPose();
 
        // compute point readings
        Vector2<kt_double> rangePointsSum;
        kt_int32u beamNum = 0;
        for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++)
        {
          kt_double rangeReading = GetRangeReadings()[i];
          if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold))
          {
            kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
 
            Vector2<kt_double> point;
            point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
            point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
 
            m_UnfilteredPointReadings.push_back(point);
            continue;
          }
 
          kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
 
          Vector2<kt_double> 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
        kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
        if (nPoints != 0.0)
        {
          Vector2<kt_double> averagePosition = Vector2<kt_double>(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());
        forEach(PointVectorDouble, &m_PointReadings)
        {
          m_BoundingBox.Add(*iter);
        }
      }
 
      m_IsDirty = false;
    }
 
  private:
    LocalizedRangeScan(const LocalizedRangeScan&);
    const LocalizedRangeScan& operator=(const LocalizedRangeScan&);
 
  private:
    Pose2 m_OdometricPose;
 
    Pose2 m_CorrectedPose;
 
  protected:
    Pose2 m_BarycenterPose;
 
    PointVectorDouble m_PointReadings;
 
    PointVectorDouble m_UnfilteredPointReadings;
 
    BoundingBox2 m_BoundingBox;
 
    kt_bool m_IsDirty;
  };  // LocalizedRangeScan
 
  typedef std::vector<LocalizedRangeScan*> LocalizedRangeScanVector;
 
 
  class LocalizedRangeScanWithPoints : public LocalizedRangeScan
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(LocalizedRangeScanWithPoints)
    // @endcond
 
  public:
    LocalizedRangeScanWithPoints(const Name& rSensorName, const RangeReadingsVector& rReadings,
        const PointVectorDouble& rPoints)
        : LocalizedRangeScan(rSensorName, rReadings),
          m_Points(rPoints)
    {
    }
 
    virtual ~LocalizedRangeScanWithPoints()
    {
    }
 
  private:
    void Update()
    {
      m_PointReadings.clear();
      m_UnfilteredPointReadings.clear();
 
      Pose2 scanPose = GetSensorPose();
      Pose2 robotPose = GetCorrectedPose();
 
      // update point readings
      Vector2<kt_double> rangePointsSum;
      for (kt_int32u i = 0; i < m_Points.size(); i++)
      {
        // check if this has a NaN
        if (!std::isfinite(m_Points[i].GetX()) || !std::isfinite(m_Points[i].GetY()))
        {
          Vector2<kt_double> point(m_Points[i].GetX(), m_Points[i].GetY());
          m_UnfilteredPointReadings.push_back(point);
 
          continue;
        }
 
        // transform into world coords
        Pose2 pointPose(m_Points[i].GetX(), m_Points[i].GetY(), 0);
        Pose2 result = Transform(robotPose).TransformPose(pointPose);
        Vector2<kt_double> point(result.GetX(), result.GetY());
 
        m_PointReadings.push_back(point);
        m_UnfilteredPointReadings.push_back(point);
 
        rangePointsSum += point;
      }
 
      // compute barycenter
      kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
      if (nPoints != 0.0)
      {
        Vector2<kt_double> averagePosition = Vector2<kt_double>(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());
      forEach(PointVectorDouble, &m_PointReadings)
      {
        m_BoundingBox.Add(*iter);
      }
 
      m_IsDirty = false;
    }
 
  private:
    LocalizedRangeScanWithPoints(const LocalizedRangeScanWithPoints&);
    const LocalizedRangeScanWithPoints& operator=(const LocalizedRangeScanWithPoints&);
 
  private:
    const PointVectorDouble m_Points;
  };  // LocalizedRangeScanWithPoints
 
 
  class OccupancyGrid;
 
  class KARTO_EXPORT CellUpdater : public Functor
  {
  public:
    CellUpdater(OccupancyGrid* pGrid)
      : m_pOccupancyGrid(pGrid)
    {
    }
 
    virtual ~CellUpdater() {}
 
    virtual void operator() (kt_int32u index);
 
  private:
    OccupancyGrid* m_pOccupancyGrid;
  };  // CellUpdater
 
  class OccupancyGrid : public Grid<kt_int8u>
  {
    friend class CellUpdater;
    friend class IncrementalOccupancyGrid;
 
  public:
    OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2<kt_double>& rOffset, kt_double resolution)
      : Grid<kt_int8u>(width, height)
      , m_pCellPassCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
      , m_pCellHitsCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
      , m_pCellUpdater(NULL)
    {
      m_pCellUpdater = new CellUpdater(this);
 
      if (karto::math::DoubleEqual(resolution, 0.0))
      {
        throw Exception("Resolution cannot be 0");
      }
 
      m_pMinPassThrough = new Parameter<kt_int32u>("MinPassThrough", 2);
      m_pOccupancyThreshold = new Parameter<kt_double>("OccupancyThreshold", 0.1);
 
      GetCoordinateConverter()->SetScale(1.0 / resolution);
      GetCoordinateConverter()->SetOffset(rOffset);
    }
 
    virtual ~OccupancyGrid()
    {
      delete m_pCellUpdater;
 
      delete m_pCellPassCnt;
      delete m_pCellHitsCnt;
 
      delete m_pMinPassThrough;
      delete m_pOccupancyThreshold;
    }
 
  public:
    static OccupancyGrid* CreateFromScans(const LocalizedRangeScanVector& rScans, kt_double resolution)
    {
      if (rScans.empty())
      {
        return NULL;
      }
 
      kt_int32s width, height;
      Vector2<kt_double> offset;
      ComputeDimensions(rScans, resolution, width, height, offset);
      OccupancyGrid* pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);
      pOccupancyGrid->CreateFromScans(rScans);
 
      return pOccupancyGrid;
    }
 
    OccupancyGrid* Clone() const
    {
      OccupancyGrid* pOccupancyGrid = new OccupancyGrid(GetWidth(),
                                                        GetHeight(),
                                                        GetCoordinateConverter()->GetOffset(),
                                                        1.0 / GetCoordinateConverter()->GetScale());
      memcpy(pOccupancyGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
 
      pOccupancyGrid->GetCoordinateConverter()->SetSize(GetCoordinateConverter()->GetSize());
      pOccupancyGrid->m_pCellPassCnt = m_pCellPassCnt->Clone();
      pOccupancyGrid->m_pCellHitsCnt = m_pCellHitsCnt->Clone();
 
      return pOccupancyGrid;
    }
 
    virtual kt_bool IsFree(const Vector2<kt_int32s>& rPose) const
    {
      kt_int8u* pOffsets = reinterpret_cast<kt_int8u*>(GetDataPointer(rPose));
      if (*pOffsets == GridStates_Free)
      {
        return true;
      }
 
      return false;
    }
 
    virtual kt_double RayCast(const Pose2& rPose2, kt_double maxRange) const
    {
      double scale = GetCoordinateConverter()->GetScale();
 
      kt_double x = rPose2.GetX();
      kt_double y = rPose2.GetY();
      kt_double theta = rPose2.GetHeading();
 
      kt_double sinTheta = sin(theta);
      kt_double cosTheta = cos(theta);
 
      kt_double xStop = x + maxRange * cosTheta;
      kt_double xSteps = 1 + fabs(xStop - x) * scale;
 
      kt_double yStop = y + maxRange * sinTheta;
      kt_double ySteps = 1 + fabs(yStop - y) * scale;
 
      kt_double steps = math::Maximum(xSteps, ySteps);
      kt_double delta = maxRange / steps;
      kt_double distance = delta;
 
      for (kt_int32u i = 1; i < steps; i++)
      {
        kt_double x1 = x + distance * cosTheta;
        kt_double y1 = y + distance * sinTheta;
 
        Vector2<kt_int32s> gridIndex = WorldToGrid(Vector2<kt_double>(x1, y1));
        if (IsValidGridIndex(gridIndex) && IsFree(gridIndex))
        {
          distance = (i + 1) * delta;
        }
        else
        {
          break;
        }
      }
 
      return (distance < maxRange)? distance : maxRange;
    }
 
    void SetMinPassThrough(kt_int32u count)
    {
      m_pMinPassThrough->SetValue(count);
    }
 
    void SetOccupancyThreshold(kt_double thresh)
    {
      m_pOccupancyThreshold->SetValue(thresh);
    }
 
  protected:
    virtual Grid<kt_int32u>* GetCellHitsCounts()
    {
      return m_pCellHitsCnt;
    }
 
    virtual Grid<kt_int32u>* GetCellPassCounts()
    {
      return m_pCellPassCnt;
    }
 
  protected:
    static void ComputeDimensions(const LocalizedRangeScanVector& rScans,
                                  kt_double resolution,
                                  kt_int32s& rWidth,
                                  kt_int32s& rHeight,
                                  Vector2<kt_double>& rOffset)
    {
      BoundingBox2 boundingBox;
      const_forEach(LocalizedRangeScanVector, &rScans)
      {
        boundingBox.Add((*iter)->GetBoundingBox());
      }
 
      kt_double scale = 1.0 / resolution;
      Size2<kt_double> size = boundingBox.GetSize();
 
      rWidth = static_cast<kt_int32s>(math::Round(size.GetWidth() * scale));
      rHeight = static_cast<kt_int32s>(math::Round(size.GetHeight() * scale));
      rOffset = boundingBox.GetMinimum();
    }
 
    virtual void CreateFromScans(const LocalizedRangeScanVector& rScans)
    {
      m_pCellPassCnt->Resize(GetWidth(), GetHeight());
      m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
 
      m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
      m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
 
      const_forEach(LocalizedRangeScanVector, &rScans)
      {
        LocalizedRangeScan* pScan = *iter;
        AddScan(pScan);
      }
 
      Update();
    }
 
    virtual kt_bool AddScan(LocalizedRangeScan* pScan, kt_bool doUpdate = false)
    {
      kt_double rangeThreshold = pScan->GetLaserRangeFinder()->GetRangeThreshold();
      kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
      kt_double minRange = pScan->GetLaserRangeFinder()->GetMinimumRange();
 
      Vector2<kt_double> scanPosition = pScan->GetSensorPose().GetPosition();
 
      // get scan point readings
      const PointVectorDouble& rPointReadings = pScan->GetPointReadings(false);
 
      kt_bool isAllInMap = true;
 
      // draw lines from scan position to all point readings
      int pointIndex = 0;
      const_forEachAs(PointVectorDouble, &rPointReadings, pointsIter)
      {
        Vector2<kt_double> point = *pointsIter;
        kt_double rangeReading = pScan->GetRangeReadings()[pointIndex];
        kt_bool isEndPointValid = rangeReading < (rangeThreshold - KT_TOLERANCE);
 
        if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading))
        {
          // ignore these readings
          pointIndex++;
          continue;
        }
        else if (rangeReading >= rangeThreshold)
        {
          // trace up to range reading
          kt_double ratio = rangeThreshold / rangeReading;
          kt_double dx = point.GetX() - scanPosition.GetX();
          kt_double dy = point.GetY() - scanPosition.GetY();
          point.SetX(scanPosition.GetX() + ratio * dx);
          point.SetY(scanPosition.GetY() + ratio * dy);
        }
 
        kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate);
        if (!isInMap)
        {
          isAllInMap = false;
        }
 
        pointIndex++;
      }
 
      return isAllInMap;
    }
 
    virtual kt_bool RayTrace(const Vector2<kt_double>& rWorldFrom,
                             const Vector2<kt_double>& rWorldTo,
                             kt_bool isEndPointValid,
                             kt_bool doUpdate = false)
    {
      assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
 
      Vector2<kt_int32s> gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom);
      Vector2<kt_int32s> gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo);
 
      CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL;
      m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater);
 
      // for the end point
      if (isEndPointValid)
      {
        if (m_pCellPassCnt->IsValidGridIndex(gridTo))
        {
          kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false);
 
          kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
          kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
 
          // increment cell pass through and hit count
          pCellPassCntPtr[index]++;
          pCellHitCntPtr[index]++;
 
          if (doUpdate)
          {
            (*m_pCellUpdater)(index);
          }
        }
      }
 
      return m_pCellPassCnt->IsValidGridIndex(gridTo);
    }
 
    virtual void UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
    {
      if (cellPassCnt > m_pMinPassThrough->GetValue())
      {
        kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);
 
        if (hitRatio > m_pOccupancyThreshold->GetValue())
        {
          *pCell = GridStates_Occupied;
        }
        else
        {
          *pCell = GridStates_Free;
        }
      }
    }
 
    virtual void Update()
    {
      assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
 
      // clear grid
      Clear();
 
      // set occupancy status of cells
      kt_int8u* pDataPtr = GetDataPointer();
      kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
      kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
 
      kt_int32u nBytes = GetDataSize();
      for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++)
      {
        UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);
      }
    }
 
    virtual void Resize(kt_int32s width, kt_int32s height)
    {
      Grid<kt_int8u>::Resize(width, height);
      m_pCellPassCnt->Resize(width, height);
      m_pCellHitsCnt->Resize(width, height);
    }
 
  protected:
    Grid<kt_int32u>* m_pCellPassCnt;
 
    Grid<kt_int32u>* m_pCellHitsCnt;
 
  private:
    OccupancyGrid(const OccupancyGrid&);
 
    const OccupancyGrid& operator=(const OccupancyGrid&);
 
  private:
    CellUpdater* m_pCellUpdater;
 
    // NOTE: These two values are dependent on the resolution.  If the resolution is too small,
    // then not many beams will hit the cell!
 
    // Number of beams that must pass through a cell before it will be considered to be occupied
    // or unoccupied.  This prevents stray beams from messing up the map.
    Parameter<kt_int32u>* m_pMinPassThrough;
 
    // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied
    Parameter<kt_double>* m_pOccupancyThreshold;
  };  // OccupancyGrid
 
 
  class DatasetInfo : public Object
  {
  public:
    // @cond EXCLUDE
    KARTO_Object(DatasetInfo)
    // @endcond
 
  public:
    DatasetInfo()
      : Object()
    {
      m_pTitle = new Parameter<std::string>("Title", "", GetParameterManager());
      m_pAuthor = new Parameter<std::string>("Author", "", GetParameterManager());
      m_pDescription = new Parameter<std::string>("Description", "", GetParameterManager());
      m_pCopyright = new Parameter<std::string>("Copyright", "", GetParameterManager());
    }
 
    virtual ~DatasetInfo()
    {
    }
 
  public:
    const std::string& GetTitle() const
    {
      return m_pTitle->GetValue();
    }
 
    const std::string& GetAuthor() const
    {
      return m_pAuthor->GetValue();
    }
 
    const std::string& GetDescription() const
    {
      return m_pDescription->GetValue();
    }
 
    const std::string& GetCopyright() const
    {
      return m_pCopyright->GetValue();
    }
 
  private:
    DatasetInfo(const DatasetInfo&);
    const DatasetInfo& operator=(const DatasetInfo&);
 
  private:
    Parameter<std::string>* m_pTitle;
    Parameter<std::string>* m_pAuthor;
    Parameter<std::string>* m_pDescription;
    Parameter<std::string>* m_pCopyright;
  };  // class DatasetInfo
 
 
  class Dataset
  {
  public:
    Dataset()
      : m_pDatasetInfo(NULL)
    {
    }
 
    virtual ~Dataset()
    {
      Clear();
    }
 
  public:
    void Add(Object* pObject)
    {
      if (pObject != NULL)
      {
        if (dynamic_cast<Sensor*>(pObject))
        {
          Sensor* pSensor = dynamic_cast<Sensor*>(pObject);
          if (pSensor != NULL)
          {
            m_SensorNameLookup[pSensor->GetName()] = pSensor;
 
            karto::SensorManager::GetInstance()->RegisterSensor(pSensor);
          }
 
          m_Objects.push_back(pObject);
        }
        else if (dynamic_cast<SensorData*>(pObject))
        {
          SensorData* pSensorData = dynamic_cast<SensorData*>(pObject);
          m_Objects.push_back(pSensorData);
        }
        else if (dynamic_cast<DatasetInfo*>(pObject))
        {
          m_pDatasetInfo = dynamic_cast<DatasetInfo*>(pObject);
        }
        else
        {
          m_Objects.push_back(pObject);
        }
      }
    }
 
    inline const ObjectVector& GetObjects() const
    {
      return m_Objects;
    }
 
    inline DatasetInfo* GetDatasetInfo()
    {
      return m_pDatasetInfo;
    }
 
    virtual void Clear()
    {
      for (std::map<Name, Sensor*>::iterator iter = m_SensorNameLookup.begin(); iter != m_SensorNameLookup.end(); ++iter)
      {
        karto::SensorManager::GetInstance()->UnregisterSensor(iter->second);
      }
 
      forEach(ObjectVector, &m_Objects)
      {
        delete *iter;
      }
 
      m_Objects.clear();
 
      if (m_pDatasetInfo != NULL)
      {
        delete m_pDatasetInfo;
        m_pDatasetInfo = NULL;
      }
    }
 
  private:
    std::map<Name, Sensor*> m_SensorNameLookup;
    ObjectVector m_Objects;
    DatasetInfo* m_pDatasetInfo;
  };  // Dataset
 
 
  class LookupArray
  {
  public:
    LookupArray()
      : m_pArray(NULL)
      , m_Capacity(0)
      , m_Size(0)
    {
    }
 
    virtual ~LookupArray()
    {
      assert(m_pArray != NULL);
 
      delete[] m_pArray;
      m_pArray = NULL;
    }
 
  public:
    void Clear()
    {
      memset(m_pArray, 0, sizeof(kt_int32s) * m_Capacity);
    }
 
    kt_int32u GetSize() const
    {
      return m_Size;
    }
 
    void SetSize(kt_int32u size)
    {
      assert(size != 0);
 
      if (size > m_Capacity)
      {
        if (m_pArray != NULL)
        {
          delete [] m_pArray;
        }
        m_Capacity = size;
        m_pArray = new kt_int32s[m_Capacity];
      }
 
      m_Size = size;
    }
 
    inline kt_int32s& operator [] (kt_int32u index)
    {
      assert(index < m_Size);
 
      return m_pArray[index];
    }
 
    inline kt_int32s operator [] (kt_int32u index) const
    {
      assert(index < m_Size);
 
      return m_pArray[index];
    }
 
    inline kt_int32s* GetArrayPointer()
    {
      return m_pArray;
    }
 
    inline kt_int32s* GetArrayPointer() const
    {
      return m_pArray;
    }
 
  private:
    kt_int32s* m_pArray;
    kt_int32u m_Capacity;
    kt_int32u m_Size;
  };  // LookupArray
 
 
  template<typename T>
  class GridIndexLookup
  {
  public:
    GridIndexLookup(Grid<T>* pGrid)
      : m_pGrid(pGrid)
      , m_Capacity(0)
      , m_Size(0)
      , m_ppLookupArray(NULL)
    {
    }
 
    virtual ~GridIndexLookup()
    {
      DestroyArrays();
    }
 
  public:
    const LookupArray* GetLookupArray(kt_int32u index) const
    {
      assert(math::IsUpTo(index, m_Size));
 
      return m_ppLookupArray[index];
    }
 
    const std::vector<kt_double>& GetAngles() const
    {
      return m_Angles;
    }
 
    void ComputeOffsets(LocalizedRangeScan* pScan,
                        kt_double angleCenter,
                        kt_double angleOffset,
                        kt_double angleResolution)
    {
      assert(angleOffset != 0.0);
      assert(angleResolution != 0.0);
 
      kt_int32u nAngles = static_cast<kt_int32u>(math::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->GetSensorPose());
 
      Pose2Vector localPoints;
      const_forEach(PointVectorDouble, &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
      kt_double angle = 0.0;
      kt_double startAngle = angleCenter - angleOffset;
      for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
      {
        angle = startAngle + angleIndex * angleResolution;
        ComputeOffsets(angleIndex, angle, localPoints, pScan);
      }
      // assert(math::DoubleEqual(angle, angleCenter + angleOffset));
    }
 
  private:
    void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2Vector& rLocalPoints, LocalizedRangeScan* pScan)
    {
      m_ppLookupArray[angleIndex]->SetSize(static_cast<kt_int32u>(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<kt_double>& rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset();
 
      kt_double cosine = cos(angle);
      kt_double sine = sin(angle);
 
      kt_int32u readingIndex = 0;
 
      kt_int32s* pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer();
 
      kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
 
      const_forEach(Pose2Vector, &rLocalPoints)
      {
        const Vector2<kt_double>& 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<kt_double> 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<kt_int32s> gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset);
 
        // use base GridIndex to ignore ROI
        kt_int32s lookupIndex = m_pGrid->Grid<T>::GridIndex(gridPoint, false);
 
        pAngleIndexPointer[readingIndex] = lookupIndex;
 
        readingIndex++;
      }
      assert(readingIndex == rLocalPoints.size());
    }
 
    void SetSize(kt_int32u size)
    {
      assert(size != 0);
 
      if (size > m_Capacity)
      {
        if (m_ppLookupArray != NULL)
        {
          DestroyArrays();
        }
 
        m_Capacity = size;
        m_ppLookupArray = new LookupArray*[m_Capacity];
        for (kt_int32u i = 0; i < m_Capacity; i++)
        {
          m_ppLookupArray[i] = new LookupArray();
        }
      }
 
      m_Size = size;
 
      m_Angles.resize(size);
    }
 
    void DestroyArrays()
    {
      for (kt_int32u i = 0; i < m_Capacity; i++)
      {
        delete m_ppLookupArray[i];
      }
 
      delete[] m_ppLookupArray;
      m_ppLookupArray = NULL;
    }
 
  private:
    Grid<T>* m_pGrid;
 
    kt_int32u m_Capacity;
    kt_int32u m_Size;
 
    LookupArray **m_ppLookupArray;
 
    // for sanity check
    std::vector<kt_double> m_Angles;
  };  // class GridIndexLookup
 
 
  inline Pose2::Pose2(const Pose3& rPose)
    : m_Position(rPose.GetPosition().GetX(), rPose.GetPosition().GetY())
  {
    kt_double t1, t2;
 
    // calculates heading from orientation
    rPose.GetOrientation().ToEulerAngles(m_Heading, t1, t2);
  }
 
 
  // @cond EXCLUDE
 
  template<typename T>
  inline void Object::SetParameter(const std::string& rName, T value)
  {
    AbstractParameter* pParameter = GetParameter(rName);
    if (pParameter != NULL)
    {
      std::stringstream stream;
      stream << value;
      pParameter->SetValueFromString(stream.str());
    }
    else
    {
      throw Exception("Parameter does not exist:  " + rName);
    }
  }
 
  template<>
  inline void Object::SetParameter(const std::string& rName, kt_bool value)
  {
    AbstractParameter* pParameter = GetParameter(rName);
    if (pParameter != NULL)
    {
      pParameter->SetValueFromString(value ? "true" : "false");
    }
    else
    {
      throw Exception("Parameter does not exist:  " + rName);
    }
  }
 
  // @endcond
 
}  // namespace karto
 
#endif  // OPEN_KARTO_KARTO_H