Karto.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2010 SRI International
00003  *
00004  * This program is free software: you can redistribute it and/or modify
00005  * it under the terms of the GNU Lesser General Public License as published by
00006  * the Free Software Foundation, either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 #ifndef OPEN_KARTO_KARTO_H
00019 #define OPEN_KARTO_KARTO_H
00020 
00021 #include <string>
00022 #include <fstream>
00023 #include <limits>
00024 #include <algorithm>
00025 #include <map>
00026 #include <vector>
00027 #include <iostream>
00028 #include <iomanip>
00029 #include <sstream>
00030 #include <stdexcept>
00031 
00032 #include <math.h>
00033 #include <float.h>
00034 #include <stdio.h>
00035 #include <assert.h>
00036 #include <string.h>
00037 #include <boost/thread.hpp>
00038 
00039 #ifdef USE_POCO
00040 #include <Poco/Mutex.h>
00041 #endif
00042 
00043 #include <open_karto/Math.h>
00044 #include <open_karto/Macros.h>
00045 
00046 #define KARTO_Object(name) \
00047   virtual const char* GetClassName() const { return #name; } \
00048   virtual kt_objecttype GetObjectType() const { return ObjectType_##name; }
00049 
00050 typedef kt_int32u kt_objecttype;
00051 
00052 const kt_objecttype ObjectType_None                         = 0x00000000;
00053 const kt_objecttype ObjectType_Sensor                       = 0x00001000;
00054 const kt_objecttype ObjectType_SensorData                   = 0x00002000;
00055 const kt_objecttype ObjectType_CustomData                   = 0x00004000;
00056 const kt_objecttype ObjectType_Misc                         = 0x10000000;
00057 
00058 const kt_objecttype ObjectType_Drive                        = ObjectType_Sensor | 0x01;
00059 const kt_objecttype ObjectType_LaserRangeFinder             = ObjectType_Sensor | 0x02;
00060 const kt_objecttype ObjectType_Camera                       = ObjectType_Sensor | 0x04;
00061 
00062 const kt_objecttype ObjectType_DrivePose                    = ObjectType_SensorData | 0x01;
00063 const kt_objecttype ObjectType_LaserRangeScan               = ObjectType_SensorData | 0x02;
00064 const kt_objecttype ObjectType_LocalizedRangeScan           = ObjectType_SensorData | 0x04;
00065 const kt_objecttype ObjectType_CameraImage                  = ObjectType_SensorData | 0x08;
00066 const kt_objecttype ObjectType_LocalizedRangeScanWithPoints = ObjectType_SensorData | 0x16;
00067 
00068 const kt_objecttype ObjectType_Header                       = ObjectType_Misc | 0x01;
00069 const kt_objecttype ObjectType_Parameters                   = ObjectType_Misc | 0x02;
00070 const kt_objecttype ObjectType_DatasetInfo                  = ObjectType_Misc | 0x04;
00071 const kt_objecttype ObjectType_Module                       = ObjectType_Misc | 0x08;
00072 
00073 namespace karto
00074 {
00075 
00080 
00084   class KARTO_EXPORT Exception
00085   {
00086   public:
00092     Exception(const std::string& rMessage = "Karto Exception", kt_int32s errorCode = 0)
00093       : m_Message(rMessage)
00094       , m_ErrorCode(errorCode)
00095     {
00096     }
00097 
00101     Exception(const Exception& rException)
00102       : m_Message(rException.m_Message)
00103       , m_ErrorCode(rException.m_ErrorCode)
00104     {
00105     }
00106 
00110     virtual ~Exception()
00111     {
00112     }
00113 
00114   public:
00118     Exception& operator = (const Exception& rException)
00119     {
00120       m_Message = rException.m_Message;
00121       m_ErrorCode = rException.m_ErrorCode;
00122 
00123       return *this;
00124     }
00125 
00126   public:
00131     const std::string& GetErrorMessage() const
00132     {
00133       return m_Message;
00134     }
00135 
00140     kt_int32s GetErrorCode()
00141     {
00142       return m_ErrorCode;
00143     }
00144 
00145   public:
00151     friend KARTO_EXPORT std::ostream& operator << (std::ostream& rStream, Exception& rException);
00152 
00153   private:
00154     std::string m_Message;
00155     kt_int32s m_ErrorCode;
00156   };  // class Exception
00157 
00161 
00166   class KARTO_EXPORT NonCopyable
00167   {
00168   private:
00169     NonCopyable(const NonCopyable&);
00170     const NonCopyable& operator=(const NonCopyable&);
00171 
00172   protected:
00173     NonCopyable()
00174     {
00175     }
00176 
00177     virtual ~NonCopyable()
00178     {
00179     }
00180   };  // class NonCopyable
00181 
00185 
00189   template <class T>
00190   class Singleton
00191   {
00192   public:
00196     Singleton()
00197       : m_pPointer(NULL)
00198     {
00199     }
00200 
00204     virtual ~Singleton()
00205     {
00206       delete m_pPointer;
00207     }
00208 
00213     T* Get()
00214     {
00215 #ifdef USE_POCO
00216       Poco::FastMutex::ScopedLock lock(m_Mutex);
00217 #endif
00218       if (m_pPointer == NULL)
00219       {
00220         m_pPointer = new T;
00221       }
00222 
00223       return m_pPointer;
00224     }
00225 
00226   private:
00227     T* m_pPointer;
00228 
00229 #ifdef USE_POCO
00230     Poco::FastMutex m_Mutex;
00231 #endif
00232 
00233   private:
00234     Singleton(const Singleton&);
00235     const Singleton& operator=(const Singleton&);
00236   };
00237 
00241 
00245   class KARTO_EXPORT Functor
00246   {
00247   public:
00251     virtual void operator() (kt_int32u) {};
00252   };  // Functor
00253 
00257 
00258   class AbstractParameter;
00259 
00263   typedef std::vector<AbstractParameter*> ParameterVector;
00264 
00268   class KARTO_EXPORT ParameterManager : public NonCopyable
00269   {
00270   public:
00274     ParameterManager()
00275     {
00276     }
00277 
00281     virtual ~ParameterManager()
00282     {
00283       Clear();
00284     }
00285 
00286   public:
00291     void Add(AbstractParameter* pParameter);
00292 
00298     AbstractParameter* Get(const std::string& rName)
00299     {
00300       if (m_ParameterLookup.find(rName) != m_ParameterLookup.end())
00301       {
00302         return m_ParameterLookup[rName];
00303       }
00304 
00305       std::cout << "Unknown parameter: " << rName << std::endl;
00306 
00307       return NULL;
00308     }
00309 
00313     void Clear();
00314 
00319     inline const ParameterVector& GetParameterVector() const
00320     {
00321       return m_Parameters;
00322     }
00323 
00324   public:
00330     AbstractParameter* operator() (const std::string& rName)
00331     {
00332       return Get(rName);
00333     }
00334 
00335   private:
00336     ParameterManager(const ParameterManager&);
00337     const ParameterManager& operator=(const ParameterManager&);
00338 
00339   private:
00340     ParameterVector m_Parameters;
00341     std::map<std::string, AbstractParameter*> m_ParameterLookup;
00342   };  // ParameterManager
00343 
00347 
00348   // valid names
00349   // 'Test' -- no scope
00350   // '/Test' -- no scope will be parsed to 'Test'
00351   // '/scope/Test' - 'scope' scope and 'Test' name
00352   // '/scope/name/Test' - 'scope/name' scope and 'Test' name
00353   //
00354   class Name
00355   {
00356   public:
00360     Name()
00361     {
00362     }
00363 
00367     Name(const std::string& rName)
00368     {
00369       Parse(rName);
00370     }
00371 
00375     Name(const Name& rOther)
00376       : m_Name(rOther.m_Name)
00377       , m_Scope(rOther.m_Scope)
00378     {
00379     }
00380 
00384     virtual ~Name()
00385     {
00386     }
00387 
00388   public:
00393     inline const std::string& GetName() const
00394     {
00395       return m_Name;
00396     }
00397 
00402     inline void SetName(const std::string& rName)
00403     {
00404       std::string::size_type pos = rName.find_last_of('/');
00405       if (pos != 0 && pos != std::string::npos)
00406       {
00407         throw Exception("Name can't contain a scope!");
00408       }
00409 
00410       m_Name = rName;
00411     }
00412 
00417     inline const std::string& GetScope() const
00418     {
00419       return m_Scope;
00420     }
00421 
00426     inline void SetScope(const std::string& rScope)
00427     {
00428       m_Scope = rScope;
00429     }
00430 
00435     inline std::string ToString() const
00436     {
00437       if (m_Scope == "")
00438       {
00439         return m_Name;
00440       }
00441       else
00442       {
00443         std::string name;
00444         name.append("/");
00445         name.append(m_Scope);
00446         name.append("/");
00447         name.append(m_Name);
00448 
00449         return name;
00450       }
00451     }
00452 
00453   public:
00457     Name& operator = (const Name& rOther)
00458     {
00459       if (&rOther != this)
00460       {
00461         m_Name = rOther.m_Name;
00462         m_Scope = rOther.m_Scope;
00463       }
00464 
00465       return *this;
00466     }
00467 
00471     kt_bool operator == (const Name& rOther) const
00472     {
00473       return (m_Name == rOther.m_Name) && (m_Scope == rOther.m_Scope);
00474     }
00475 
00479     kt_bool operator != (const Name& rOther) const
00480     {
00481       return !(*this == rOther);
00482     }
00483 
00487     kt_bool operator < (const Name& rOther) const
00488     {
00489       return ToString() < rOther.ToString();
00490     }
00491 
00497     friend inline std::ostream& operator << (std::ostream& rStream, const Name& rName)
00498     {
00499       rStream << rName.ToString();
00500       return rStream;
00501     }
00502 
00503   private:
00508     void Parse(const std::string& rName)
00509     {
00510       std::string::size_type pos = rName.find_last_of('/');
00511 
00512       if (pos == std::string::npos)
00513       {
00514         m_Name = rName;
00515       }
00516       else
00517       {
00518         m_Scope = rName.substr(0, pos);
00519         m_Name = rName.substr(pos+1, rName.size());
00520 
00521         // remove '/' from m_Scope if first!!
00522         if (m_Scope.size() > 0 && m_Scope[0] == '/')
00523         {
00524           m_Scope = m_Scope.substr(1, m_Scope.size());
00525         }
00526       }
00527     }
00528 
00533     void Validate(const std::string& rName)
00534     {
00535       if (rName.empty())
00536       {
00537         return;
00538       }
00539 
00540       char c = rName[0];
00541       if (IsValidFirst(c))
00542       {
00543         for (size_t i = 1; i < rName.size(); ++i)
00544         {
00545           c = rName[i];
00546           if (!IsValid(c))
00547           {
00548             throw Exception("Invalid character in name. Valid characters must be within the ranges A-Z, a-z, 0-9, '/', '_' and '-'.");
00549           }
00550         }
00551       }
00552       else
00553       {
00554         throw Exception("Invalid first character in name. Valid characters must be within the ranges A-Z, a-z, and '/'.");
00555       }
00556     }
00557 
00563     inline kt_bool IsValidFirst(char c)
00564     {
00565       return (isalpha(c) || c == '/');
00566     }
00567 
00573     inline kt_bool IsValid(char c)
00574     {
00575       return (isalnum(c) || c == '/' || c == '_' || c == '-');
00576     }
00577 
00578   private:
00579     std::string m_Name;
00580     std::string m_Scope;
00581   };
00582 
00586 
00590   class KARTO_EXPORT Object : public NonCopyable
00591   {
00592   public:
00596     Object();
00597 
00602     Object(const Name& rName);
00603 
00607     virtual ~Object();
00608 
00609   public:
00614     inline const Name& GetName() const
00615     {
00616       return m_Name;
00617     }
00618 
00623     virtual const char* GetClassName() const = 0;
00624 
00629     virtual kt_objecttype GetObjectType() const = 0;
00630 
00635     virtual inline ParameterManager* GetParameterManager()
00636     {
00637       return m_pParameterManager;
00638     }
00639 
00645     inline AbstractParameter* GetParameter(const std::string& rName) const
00646     {
00647       return m_pParameterManager->Get(rName);
00648     }
00649 
00655     template<typename T>
00656     inline void SetParameter(const std::string& rName, T value);
00657 
00662     inline const ParameterVector& GetParameters() const
00663     {
00664       return m_pParameterManager->GetParameterVector();
00665     }
00666 
00667   private:
00668     Object(const Object&);
00669     const Object& operator=(const Object&);
00670 
00671   private:
00672     Name m_Name;
00673     ParameterManager* m_pParameterManager;
00674   };
00675 
00679   typedef std::vector<Object*> ObjectVector;
00680 
00684 
00690   inline kt_bool IsSensor(Object* pObject)
00691   {
00692     return (pObject->GetObjectType() & ObjectType_Sensor) == ObjectType_Sensor;
00693   }
00694 
00700   inline kt_bool IsSensorData(Object* pObject)
00701   {
00702     return (pObject->GetObjectType() & ObjectType_SensorData) == ObjectType_SensorData;
00703   }
00704 
00710   inline kt_bool IsLaserRangeFinder(Object* pObject)
00711   {
00712     return (pObject->GetObjectType() & ObjectType_LaserRangeFinder) == ObjectType_LaserRangeFinder;
00713   }
00714 
00720   inline kt_bool IsLocalizedRangeScan(Object* pObject)
00721   {
00722     return (pObject->GetObjectType() & ObjectType_LocalizedRangeScan) == ObjectType_LocalizedRangeScan;
00723   }
00724 
00730   inline kt_bool IsLocalizedRangeScanWithPoints(Object* pObject)
00731   {
00732     return (pObject->GetObjectType() & ObjectType_LocalizedRangeScanWithPoints) == ObjectType_LocalizedRangeScanWithPoints;
00733   }
00734 
00740   inline kt_bool IsParameters(Object* pObject)
00741   {
00742     return (pObject->GetObjectType() & ObjectType_Parameters) == ObjectType_Parameters;
00743   }
00744 
00750   inline kt_bool IsDatasetInfo(Object* pObject)
00751   {
00752     return (pObject->GetObjectType() & ObjectType_DatasetInfo) == ObjectType_DatasetInfo;
00753   }
00754 
00758 
00762   class KARTO_EXPORT Module : public Object
00763   {
00764   public:
00765     // @cond EXCLUDE
00766     KARTO_Object(Module)
00767     // @endcond
00768 
00769   public:
00774     Module(const std::string& rName);
00775 
00779     virtual ~Module();
00780 
00781   public:
00785     virtual void Reset() = 0;
00786 
00790     virtual kt_bool Process(karto::Object*)
00791     {
00792       return false;
00793     }
00794 
00795   private:
00796     Module(const Module&);
00797     const Module& operator=(const Module&);
00798   };
00799 
00803 
00807   template<typename T>
00808   class Size2
00809   {
00810   public:
00814     Size2()
00815       : m_Width(0)
00816       , m_Height(0)
00817     {
00818     }
00819 
00825     Size2(T width, T height)
00826       : m_Width(width)
00827       , m_Height(height)
00828     {
00829     }
00830 
00835     Size2(const Size2& rOther)
00836       : m_Width(rOther.m_Width)
00837       , m_Height(rOther.m_Height)
00838     {
00839     }
00840 
00841   public:
00846     inline const T GetWidth() const
00847     {
00848       return m_Width;
00849     }
00850 
00855     inline void SetWidth(T width)
00856     {
00857       m_Width = width;
00858     }
00859 
00864     inline const T GetHeight() const
00865     {
00866       return m_Height;
00867     }
00868 
00873     inline void SetHeight(T height)
00874     {
00875       m_Height = height;
00876     }
00877 
00881     inline Size2& operator = (const Size2& rOther)
00882     {
00883       m_Width = rOther.m_Width;
00884       m_Height = rOther.m_Height;
00885 
00886       return(*this);
00887     }
00888 
00892     inline kt_bool operator == (const Size2& rOther) const
00893     {
00894       return (m_Width == rOther.m_Width && m_Height == rOther.m_Height);
00895     }
00896 
00900     inline kt_bool operator != (const Size2& rOther) const
00901     {
00902       return (m_Width != rOther.m_Width || m_Height != rOther.m_Height);
00903     }
00904 
00910     friend inline std::ostream& operator << (std::ostream& rStream, const Size2& rSize)
00911     {
00912       rStream << "(" << rSize.m_Width << ", " << rSize.m_Height << ")";
00913       return rStream;
00914     }
00915 
00916   private:
00917     T m_Width;
00918     T m_Height;
00919   };  // Size2<T>
00920 
00924 
00928   template<typename T>
00929   class Vector2
00930   {
00931   public:
00935     Vector2()
00936     {
00937       m_Values[0] = 0;
00938       m_Values[1] = 0;
00939     }
00940 
00946     Vector2(T x, T y)
00947     {
00948       m_Values[0] = x;
00949       m_Values[1] = y;
00950     }
00951 
00952   public:
00957     inline const T& GetX() const
00958     {
00959       return m_Values[0];
00960     }
00961 
00966     inline void SetX(const T& x)
00967     {
00968       m_Values[0] = x;
00969     }
00970 
00975     inline const T& GetY() const
00976     {
00977       return m_Values[1];
00978     }
00979 
00984     inline void SetY(const T& y)
00985     {
00986       m_Values[1] = y;
00987     }
00988 
00993     inline void MakeFloor(const Vector2& rOther)
00994     {
00995       if ( rOther.m_Values[0] < m_Values[0] ) m_Values[0] = rOther.m_Values[0];
00996       if ( rOther.m_Values[1] < m_Values[1] ) m_Values[1] = rOther.m_Values[1];
00997     }
00998 
01003     inline void MakeCeil(const Vector2& rOther)
01004     {
01005       if ( rOther.m_Values[0] > m_Values[0] ) m_Values[0] = rOther.m_Values[0];
01006       if ( rOther.m_Values[1] > m_Values[1] ) m_Values[1] = rOther.m_Values[1];
01007     }
01008 
01013     inline kt_double SquaredLength() const
01014     {
01015       return math::Square(m_Values[0]) + math::Square(m_Values[1]);
01016     }
01017 
01022     inline kt_double Length() const
01023     {
01024       return sqrt(SquaredLength());
01025     }
01026 
01031     inline kt_double SquaredDistance(const Vector2& rOther) const
01032     {
01033       return (*this - rOther).SquaredLength();
01034     }
01035 
01041     inline kt_double Distance(const Vector2& rOther) const
01042     {
01043       return sqrt(SquaredDistance(rOther));
01044     }
01045 
01046   public:
01050     inline void operator += (const Vector2& rOther)
01051     {
01052       m_Values[0] += rOther.m_Values[0];
01053       m_Values[1] += rOther.m_Values[1];
01054     }
01055 
01059     inline void operator -= (const Vector2& rOther)
01060     {
01061       m_Values[0] -= rOther.m_Values[0];
01062       m_Values[1] -= rOther.m_Values[1];
01063     }
01064 
01070     inline const Vector2 operator + (const Vector2& rOther) const
01071     {
01072       return Vector2(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1]);
01073     }
01074 
01080     inline const Vector2 operator - (const Vector2& rOther) const
01081     {
01082       return Vector2(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1]);
01083     }
01084 
01089     inline void operator /= (T scalar)
01090     {
01091       m_Values[0] /= scalar;
01092       m_Values[1] /= scalar;
01093     }
01094 
01100     inline const Vector2 operator / (T scalar) const
01101     {
01102       return Vector2(m_Values[0] / scalar, m_Values[1] / scalar);
01103     }
01104 
01110     inline kt_double operator * (const Vector2& rOther) const
01111     {
01112       return m_Values[0] * rOther.m_Values[0] + m_Values[1] * rOther.m_Values[1];
01113     }
01114 
01119     inline const Vector2 operator * (T scalar) const
01120     {
01121       return Vector2(m_Values[0] * scalar, m_Values[1] * scalar);
01122     }
01123 
01128     inline const Vector2 operator - (T scalar) const
01129     {
01130       return Vector2(m_Values[0] - scalar, m_Values[1] - scalar);
01131     }
01132 
01137     inline void operator *= (T scalar)
01138     {
01139       m_Values[0] *= scalar;
01140       m_Values[1] *= scalar;
01141     }
01142 
01147     inline kt_bool operator == (const Vector2& rOther) const
01148     {
01149       return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1]);
01150     }
01151 
01156     inline kt_bool operator != (const Vector2& rOther) const
01157     {
01158       return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1]);
01159     }
01160 
01166     inline kt_bool operator < (const Vector2& rOther) const
01167     {
01168       if (m_Values[0] < rOther.m_Values[0])
01169         return true;
01170       else if (m_Values[0] > rOther.m_Values[0])
01171         return false;
01172       else
01173         return (m_Values[1] < rOther.m_Values[1]);
01174     }
01175 
01181     friend inline std::ostream& operator << (std::ostream& rStream, const Vector2& rVector)
01182     {
01183       rStream << rVector.GetX() << " " << rVector.GetY();
01184       return rStream;
01185     }
01186 
01191     friend inline std::istream& operator >> (std::istream& rStream, const Vector2& /*rVector*/)
01192     {
01193       // Implement me!!  TODO(lucbettaieb): What the what?  Do I need to implement this?
01194       return rStream;
01195     }
01196 
01197   private:
01198     T m_Values[2];
01199   };  // Vector2<T>
01200 
01204   typedef std::vector< Vector2<kt_double> > PointVectorDouble;
01205 
01209 
01213   template<typename T>
01214   class Vector3
01215   {
01216   public:
01220     Vector3()
01221     {
01222       m_Values[0] = 0;
01223       m_Values[1] = 0;
01224       m_Values[2] = 0;
01225     }
01226 
01233     Vector3(T x, T y, T z)
01234     {
01235       m_Values[0] = x;
01236       m_Values[1] = y;
01237       m_Values[2] = z;
01238     }
01239 
01244     Vector3(const Vector3& rOther)
01245     {
01246       m_Values[0] = rOther.m_Values[0];
01247       m_Values[1] = rOther.m_Values[1];
01248       m_Values[2] = rOther.m_Values[2];
01249     }
01250 
01251   public:
01256     inline const T& GetX() const
01257     {
01258       return m_Values[0];
01259     }
01260 
01265     inline void SetX(const T& x)
01266     {
01267       m_Values[0] = x;
01268     }
01269 
01274     inline const T& GetY() const
01275     {
01276       return m_Values[1];
01277     }
01278 
01283     inline void SetY(const T& y)
01284     {
01285       m_Values[1] = y;
01286     }
01287 
01292     inline const T& GetZ() const
01293     {
01294       return m_Values[2];
01295     }
01296 
01301     inline void SetZ(const T& z)
01302     {
01303       m_Values[2] = z;
01304     }
01305 
01310     inline void MakeFloor(const Vector3& rOther)
01311     {
01312       if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
01313       if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
01314       if (rOther.m_Values[2] < m_Values[2]) m_Values[2] = rOther.m_Values[2];
01315     }
01316 
01321     inline void MakeCeil(const Vector3& rOther)
01322     {
01323       if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0];
01324       if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1];
01325       if (rOther.m_Values[2] > m_Values[2]) m_Values[2] = rOther.m_Values[2];
01326     }
01327 
01332     inline kt_double SquaredLength() const
01333     {
01334       return math::Square(m_Values[0]) + math::Square(m_Values[1]) + math::Square(m_Values[2]);
01335     }
01336 
01341     inline kt_double Length() const
01342     {
01343       return sqrt(SquaredLength());
01344     }
01345 
01350     inline std::string ToString() const
01351     {
01352       std::stringstream converter;
01353       converter.precision(std::numeric_limits<double>::digits10);
01354 
01355       converter << GetX() << " " << GetY() << " " << GetZ();
01356 
01357       return converter.str();
01358     }
01359 
01360   public:
01364     inline Vector3& operator = (const Vector3& rOther)
01365     {
01366       m_Values[0] = rOther.m_Values[0];
01367       m_Values[1] = rOther.m_Values[1];
01368       m_Values[2] = rOther.m_Values[2];
01369 
01370       return *this;
01371     }
01372 
01378     inline const Vector3 operator + (const Vector3& rOther) const
01379     {
01380       return Vector3(m_Values[0] + rOther.m_Values[0],
01381                      m_Values[1] + rOther.m_Values[1],
01382                      m_Values[2] + rOther.m_Values[2]);
01383     }
01384 
01390     inline const Vector3 operator + (kt_double scalar) const
01391     {
01392       return Vector3(m_Values[0] + scalar,
01393                      m_Values[1] + scalar,
01394                      m_Values[2] + scalar);
01395     }
01396 
01402     inline const Vector3 operator - (const Vector3& rOther) const
01403     {
01404       return Vector3(m_Values[0] - rOther.m_Values[0],
01405                      m_Values[1] - rOther.m_Values[1],
01406                      m_Values[2] - rOther.m_Values[2]);
01407     }
01408 
01414     inline const Vector3 operator - (kt_double scalar) const
01415     {
01416       return Vector3(m_Values[0] - scalar, m_Values[1] - scalar, m_Values[2] - scalar);
01417     }
01418 
01423     inline const Vector3 operator * (T scalar) const
01424     {
01425       return Vector3(m_Values[0] * scalar, m_Values[1] * scalar, m_Values[2] * scalar);
01426     }
01427 
01432     inline kt_bool operator == (const Vector3& rOther) const
01433     {
01434       return (m_Values[0] == rOther.m_Values[0] &&
01435               m_Values[1] == rOther.m_Values[1] &&
01436               m_Values[2] == rOther.m_Values[2]);
01437     }
01438 
01443     inline kt_bool operator != (const Vector3& rOther) const
01444     {
01445       return (m_Values[0] != rOther.m_Values[0] ||
01446               m_Values[1] != rOther.m_Values[1] ||
01447               m_Values[2] != rOther.m_Values[2]);
01448     }
01449 
01455     friend inline std::ostream& operator << (std::ostream& rStream, const Vector3& rVector)
01456     {
01457       rStream << rVector.ToString();
01458       return rStream;
01459     }
01460 
01465     friend inline std::istream& operator >> (std::istream& rStream, const Vector3& /*rVector*/)
01466     {
01467       // Implement me!!
01468       return rStream;
01469     }
01470 
01471   private:
01472     T m_Values[3];
01473   };  // Vector3
01474 
01478 
01500   class Quaternion
01501   {
01502   public:
01506     inline Quaternion()
01507     {
01508       m_Values[0] = 0.0;
01509       m_Values[1] = 0.0;
01510       m_Values[2] = 0.0;
01511       m_Values[3] = 1.0;
01512     }
01513 
01521     inline Quaternion(kt_double x, kt_double y, kt_double z, kt_double w)
01522     {
01523       m_Values[0] = x;
01524       m_Values[1] = y;
01525       m_Values[2] = z;
01526       m_Values[3] = w;
01527     }
01528 
01532     inline Quaternion(const Quaternion& rQuaternion)
01533     {
01534       m_Values[0] = rQuaternion.m_Values[0];
01535       m_Values[1] = rQuaternion.m_Values[1];
01536       m_Values[2] = rQuaternion.m_Values[2];
01537       m_Values[3] = rQuaternion.m_Values[3];
01538     }
01539 
01540   public:
01545     inline kt_double GetX() const
01546     {
01547       return m_Values[0];
01548     }
01549 
01554     inline void SetX(kt_double x)
01555     {
01556       m_Values[0] = x;
01557     }
01558 
01563     inline kt_double GetY() const
01564     {
01565       return m_Values[1];
01566     }
01567 
01572     inline void SetY(kt_double y)
01573     {
01574       m_Values[1] = y;
01575     }
01576 
01581     inline kt_double GetZ() const
01582     {
01583       return m_Values[2];
01584     }
01585 
01590     inline void SetZ(kt_double z)
01591     {
01592       m_Values[2] = z;
01593     }
01594 
01599     inline kt_double GetW() const
01600     {
01601       return m_Values[3];
01602     }
01603 
01608     inline void SetW(kt_double w)
01609     {
01610       m_Values[3] = w;
01611     }
01612 
01620     void ToEulerAngles(kt_double& rYaw, kt_double& rPitch, kt_double& rRoll) const
01621     {
01622       kt_double test = m_Values[0] * m_Values[1] + m_Values[2] * m_Values[3];
01623 
01624       if (test > 0.499)
01625       {
01626         // singularity at north pole
01627         rYaw = 2 * atan2(m_Values[0], m_Values[3]);;
01628         rPitch = KT_PI_2;
01629         rRoll = 0;
01630       }
01631       else if (test < -0.499)
01632       {
01633         // singularity at south pole
01634         rYaw = -2 * atan2(m_Values[0], m_Values[3]);
01635         rPitch = -KT_PI_2;
01636         rRoll = 0;
01637       }
01638       else
01639       {
01640         kt_double sqx = m_Values[0] * m_Values[0];
01641         kt_double sqy = m_Values[1] * m_Values[1];
01642         kt_double sqz = m_Values[2] * m_Values[2];
01643 
01644         rYaw = atan2(2 * m_Values[1] * m_Values[3] - 2 * m_Values[0] * m_Values[2], 1 - 2 * sqy - 2 * sqz);
01645         rPitch = asin(2 * test);
01646         rRoll = atan2(2 * m_Values[0] * m_Values[3] - 2 * m_Values[1] * m_Values[2], 1 - 2 * sqx - 2 * sqz);
01647       }
01648     }
01649 
01657     void FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll)
01658     {
01659       kt_double angle;
01660 
01661       angle = yaw * 0.5;
01662       kt_double cYaw = cos(angle);
01663       kt_double sYaw = sin(angle);
01664 
01665       angle = pitch * 0.5;
01666       kt_double cPitch = cos(angle);
01667       kt_double sPitch = sin(angle);
01668 
01669       angle = roll * 0.5;
01670       kt_double cRoll = cos(angle);
01671       kt_double sRoll = sin(angle);
01672 
01673       m_Values[0] = sYaw * sPitch * cRoll + cYaw * cPitch * sRoll;
01674       m_Values[1] = sYaw * cPitch * cRoll + cYaw * sPitch * sRoll;
01675       m_Values[2] = cYaw * sPitch * cRoll - sYaw * cPitch * sRoll;
01676       m_Values[3] = cYaw * cPitch * cRoll - sYaw * sPitch * sRoll;
01677     }
01678 
01683     inline Quaternion& operator = (const Quaternion& rQuaternion)
01684     {
01685       m_Values[0] = rQuaternion.m_Values[0];
01686       m_Values[1] = rQuaternion.m_Values[1];
01687       m_Values[2] = rQuaternion.m_Values[2];
01688       m_Values[3] = rQuaternion.m_Values[3];
01689 
01690       return(*this);
01691     }
01692 
01697     inline kt_bool operator == (const Quaternion& rOther) const
01698     {
01699       return (m_Values[0] == rOther.m_Values[0] &&
01700               m_Values[1] == rOther.m_Values[1] &&
01701               m_Values[2] == rOther.m_Values[2] &&
01702               m_Values[3] == rOther.m_Values[3]);
01703     }
01704 
01709     inline kt_bool operator != (const Quaternion& rOther) const
01710     {
01711       return (m_Values[0] != rOther.m_Values[0] ||
01712               m_Values[1] != rOther.m_Values[1] ||
01713               m_Values[2] != rOther.m_Values[2] ||
01714               m_Values[3] != rOther.m_Values[3]);
01715     }
01716 
01722     friend inline std::ostream& operator << (std::ostream& rStream, const Quaternion& rQuaternion)
01723     {
01724       rStream << rQuaternion.m_Values[0] << " "
01725               << rQuaternion.m_Values[1] << " "
01726               << rQuaternion.m_Values[2] << " "
01727               << rQuaternion.m_Values[3];
01728       return rStream;
01729     }
01730 
01731   private:
01732     kt_double m_Values[4];
01733   };
01734 
01738 
01743   template<typename T>
01744   class Rectangle2
01745   {
01746   public:
01750     Rectangle2()
01751     {
01752     }
01753 
01761     Rectangle2(T x, T y, T width, T height)
01762       : m_Position(x, y)
01763       , m_Size(width, height)
01764     {
01765     }
01766 
01772     Rectangle2(const Vector2<T>& rPosition, const Size2<T>& rSize)
01773       : m_Position(rPosition)
01774       , m_Size(rSize)
01775     {
01776     }
01777 
01781     Rectangle2(const Rectangle2& rOther)
01782       : m_Position(rOther.m_Position)
01783       , m_Size(rOther.m_Size)
01784     {
01785     }
01786 
01787   public:
01792     inline T GetX() const
01793     {
01794       return m_Position.GetX();
01795     }
01796 
01801     inline void SetX(T x)
01802     {
01803       m_Position.SetX(x);
01804     }
01805 
01810     inline T GetY() const
01811     {
01812       return m_Position.GetY();
01813     }
01814 
01819     inline void SetY(T y)
01820     {
01821       m_Position.SetY(y);
01822     }
01823 
01828     inline T GetWidth() const
01829     {
01830       return m_Size.GetWidth();
01831     }
01832 
01837     inline void SetWidth(T width)
01838     {
01839       m_Size.SetWidth(width);
01840     }
01841 
01846     inline T GetHeight() const
01847     {
01848       return m_Size.GetHeight();
01849     }
01850 
01855     inline void SetHeight(T height)
01856     {
01857       m_Size.SetHeight(height);
01858     }
01859 
01864     inline const Vector2<T>& GetPosition() const
01865     {
01866       return m_Position;
01867     }
01868 
01874     inline void SetPosition(const T& rX, const T& rY)
01875     {
01876       m_Position = Vector2<T>(rX, rY);
01877     }
01878 
01883     inline void SetPosition(const Vector2<T>& rPosition)
01884     {
01885       m_Position = rPosition;
01886     }
01887 
01892     inline const Size2<T>& GetSize() const
01893     {
01894       return m_Size;
01895     }
01896 
01901     inline void SetSize(const Size2<T>& rSize)
01902     {
01903       m_Size = rSize;
01904     }
01905 
01910     inline const Vector2<T> GetCenter() const
01911     {
01912       return Vector2<T>(m_Position.GetX() + m_Size.GetWidth() * 0.5, m_Position.GetY() + m_Size.GetHeight() * 0.5);
01913     }
01914 
01915   public:
01919     Rectangle2& operator = (const Rectangle2& rOther)
01920     {
01921       m_Position = rOther.m_Position;
01922       m_Size = rOther.m_Size;
01923 
01924       return *this;
01925     }
01926 
01930     inline kt_bool operator == (const Rectangle2& rOther) const
01931     {
01932       return (m_Position == rOther.m_Position && m_Size == rOther.m_Size);
01933     }
01934 
01938     inline kt_bool operator != (const Rectangle2& rOther) const
01939     {
01940       return (m_Position != rOther.m_Position || m_Size != rOther.m_Size);
01941     }
01942 
01943   private:
01944     Vector2<T> m_Position;
01945     Size2<T> m_Size;
01946   };  // Rectangle2
01947 
01951 
01952   class Pose3;
01953 
01957   class Pose2
01958   {
01959   public:
01963     Pose2()
01964       : m_Heading(0.0)
01965     {
01966     }
01967 
01973     Pose2(const Vector2<kt_double>& rPosition, kt_double heading)
01974       : m_Position(rPosition)
01975       , m_Heading(heading)
01976     {
01977     }
01978 
01985     Pose2(kt_double x, kt_double y, kt_double heading)
01986       : m_Position(x, y)
01987       , m_Heading(heading)
01988     {
01989     }
01990 
01994     Pose2(const Pose3& rPose);
01995 
01999     Pose2(const Pose2& rOther)
02000       : m_Position(rOther.m_Position)
02001       , m_Heading(rOther.m_Heading)
02002     {
02003     }
02004 
02005   public:
02010     inline kt_double GetX() const
02011     {
02012       return m_Position.GetX();
02013     }
02014 
02019     inline void SetX(kt_double x)
02020     {
02021       m_Position.SetX(x);
02022     }
02023 
02028     inline kt_double GetY() const
02029     {
02030       return m_Position.GetY();
02031     }
02032 
02037     inline void SetY(kt_double y)
02038     {
02039       m_Position.SetY(y);
02040     }
02041 
02046     inline const Vector2<kt_double>& GetPosition() const
02047     {
02048       return m_Position;
02049     }
02050 
02055     inline void SetPosition(const Vector2<kt_double>& rPosition)
02056     {
02057       m_Position = rPosition;
02058     }
02059 
02064     inline kt_double GetHeading() const
02065     {
02066       return m_Heading;
02067     }
02068 
02073     inline void SetHeading(kt_double heading)
02074     {
02075       m_Heading = heading;
02076     }
02077 
02082     inline kt_double SquaredDistance(const Pose2& rOther) const
02083     {
02084       return m_Position.SquaredDistance(rOther.m_Position);
02085     }
02086 
02087   public:
02091     inline Pose2& operator = (const Pose2& rOther)
02092     {
02093       m_Position = rOther.m_Position;
02094       m_Heading = rOther.m_Heading;
02095 
02096       return *this;
02097     }
02098 
02102     inline kt_bool operator == (const Pose2& rOther) const
02103     {
02104       return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading);
02105     }
02106 
02110     inline kt_bool operator != (const Pose2& rOther) const
02111     {
02112       return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading);
02113     }
02114 
02118     inline void operator += (const Pose2& rOther)
02119     {
02120       m_Position += rOther.m_Position;
02121       m_Heading = math::NormalizeAngle(m_Heading + rOther.m_Heading);
02122     }
02123 
02129     inline Pose2 operator + (const Pose2& rOther) const
02130     {
02131       return Pose2(m_Position + rOther.m_Position, math::NormalizeAngle(m_Heading + rOther.m_Heading));
02132     }
02133 
02139     inline Pose2 operator - (const Pose2& rOther) const
02140     {
02141       return Pose2(m_Position - rOther.m_Position, math::NormalizeAngle(m_Heading - rOther.m_Heading));
02142     }
02143 
02148     friend inline std::istream& operator >> (std::istream& rStream, const Pose2& /*rPose*/)
02149     {
02150       // Implement me!!
02151       return rStream;
02152     }
02153 
02159     friend inline std::ostream& operator << (std::ostream& rStream, const Pose2& rPose)
02160     {
02161       rStream << rPose.m_Position.GetX() << " " << rPose.m_Position.GetY() << " " << rPose.m_Heading;
02162       return rStream;
02163     }
02164 
02165   private:
02166     Vector2<kt_double> m_Position;
02167 
02168     kt_double m_Heading;
02169   };  // Pose2
02170 
02174   typedef std::vector< Pose2 > Pose2Vector;
02175 
02179 
02187   class Pose3
02188   {
02189   public:
02193     Pose3()
02194     {
02195     }
02196 
02201     Pose3(const Vector3<kt_double>& rPosition)
02202       : m_Position(rPosition)
02203     {
02204     }
02205 
02211     Pose3(const Vector3<kt_double>& rPosition, const karto::Quaternion& rOrientation)
02212       : m_Position(rPosition)
02213       , m_Orientation(rOrientation)
02214     {
02215     }
02216 
02220     Pose3(const Pose3& rOther)
02221       : m_Position(rOther.m_Position)
02222       , m_Orientation(rOther.m_Orientation)
02223     {
02224     }
02225 
02229     Pose3(const Pose2& rPose)
02230     {
02231       m_Position = Vector3<kt_double>(rPose.GetX(), rPose.GetY(), 0.0);
02232       m_Orientation.FromEulerAngles(rPose.GetHeading(), 0.0, 0.0);
02233     }
02234 
02235   public:
02240     inline const Vector3<kt_double>& GetPosition() const
02241     {
02242       return m_Position;
02243     }
02244 
02249     inline void SetPosition(const Vector3<kt_double>& rPosition)
02250     {
02251       m_Position = rPosition;
02252     }
02253 
02258     inline const Quaternion& GetOrientation() const
02259     {
02260       return m_Orientation;
02261     }
02262 
02267     inline void SetOrientation(const Quaternion& rOrientation)
02268     {
02269       m_Orientation = rOrientation;
02270     }
02271 
02276     inline std::string ToString()
02277     {
02278       std::stringstream converter;
02279       converter.precision(std::numeric_limits<double>::digits10);
02280 
02281       converter << GetPosition() << " " << GetOrientation();
02282 
02283       return converter.str();
02284     }
02285 
02286   public:
02290     inline Pose3& operator = (const Pose3& rOther)
02291     {
02292       m_Position = rOther.m_Position;
02293       m_Orientation = rOther.m_Orientation;
02294 
02295       return *this;
02296     }
02297 
02301     inline kt_bool operator == (const Pose3& rOther) const
02302     {
02303       return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation);
02304     }
02305 
02309     inline kt_bool operator != (const Pose3& rOther) const
02310     {
02311       return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation);
02312     }
02313 
02319     friend inline std::ostream& operator << (std::ostream& rStream, const Pose3& rPose)
02320     {
02321       rStream << rPose.GetPosition() << ", " << rPose.GetOrientation();
02322       return rStream;
02323     }
02324 
02329     friend inline std::istream& operator >> (std::istream& rStream, const Pose3& /*rPose*/)
02330     {
02331       // Implement me!!
02332       return rStream;
02333     }
02334 
02335   private:
02336     Vector3<kt_double> m_Position;
02337     Quaternion m_Orientation;
02338   };  // Pose3
02339 
02343 
02347   class Matrix3
02348   {
02349   public:
02353     Matrix3()
02354     {
02355       Clear();
02356     }
02357 
02361     inline Matrix3(const Matrix3& rOther)
02362     {
02363       memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
02364     }
02365 
02366   public:
02370     void SetToIdentity()
02371     {
02372       memset(m_Matrix, 0, 9*sizeof(kt_double));
02373 
02374       for (kt_int32s i = 0; i < 3; i++)
02375       {
02376         m_Matrix[i][i] = 1.0;
02377       }
02378     }
02379 
02383     void Clear()
02384     {
02385       memset(m_Matrix, 0, 9*sizeof(kt_double));
02386     }
02387 
02395     void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
02396     {
02397       kt_double cosRadians = cos(radians);
02398       kt_double sinRadians = sin(radians);
02399       kt_double oneMinusCos = 1.0 - cosRadians;
02400 
02401       kt_double xx = x * x;
02402       kt_double yy = y * y;
02403       kt_double zz = z * z;
02404 
02405       kt_double xyMCos = x * y * oneMinusCos;
02406       kt_double xzMCos = x * z * oneMinusCos;
02407       kt_double yzMCos = y * z * oneMinusCos;
02408 
02409       kt_double xSin = x * sinRadians;
02410       kt_double ySin = y * sinRadians;
02411       kt_double zSin = z * sinRadians;
02412 
02413       m_Matrix[0][0] = xx * oneMinusCos + cosRadians;
02414       m_Matrix[0][1] = xyMCos - zSin;
02415       m_Matrix[0][2] = xzMCos + ySin;
02416 
02417       m_Matrix[1][0] = xyMCos + zSin;
02418       m_Matrix[1][1] = yy * oneMinusCos + cosRadians;
02419       m_Matrix[1][2] = yzMCos - xSin;
02420 
02421       m_Matrix[2][0] = xzMCos - ySin;
02422       m_Matrix[2][1] = yzMCos + xSin;
02423       m_Matrix[2][2] = zz * oneMinusCos + cosRadians;
02424     }
02425 
02430     Matrix3 Transpose() const
02431     {
02432       Matrix3 transpose;
02433 
02434       for (kt_int32u row = 0; row < 3; row++)
02435       {
02436         for (kt_int32u col = 0; col < 3; col++)
02437         {
02438           transpose.m_Matrix[row][col] = m_Matrix[col][row];
02439         }
02440       }
02441 
02442       return transpose;
02443     }
02444 
02448     Matrix3 Inverse() const
02449     {
02450       Matrix3 kInverse = *this;
02451       kt_bool haveInverse = InverseFast(kInverse, 1e-14);
02452       if (haveInverse == false)
02453       {
02454         assert(false);
02455       }
02456       return kInverse;
02457     }
02458 
02463     kt_bool InverseFast(Matrix3& rkInverse, kt_double fTolerance = KT_TOLERANCE) const
02464     {
02465       // Invert a 3x3 using cofactors.  This is about 8 times faster than
02466       // the Numerical Recipes code which uses Gaussian elimination.
02467       rkInverse.m_Matrix[0][0] = m_Matrix[1][1]*m_Matrix[2][2] - m_Matrix[1][2]*m_Matrix[2][1];
02468       rkInverse.m_Matrix[0][1] = m_Matrix[0][2]*m_Matrix[2][1] - m_Matrix[0][1]*m_Matrix[2][2];
02469       rkInverse.m_Matrix[0][2] = m_Matrix[0][1]*m_Matrix[1][2] - m_Matrix[0][2]*m_Matrix[1][1];
02470       rkInverse.m_Matrix[1][0] = m_Matrix[1][2]*m_Matrix[2][0] - m_Matrix[1][0]*m_Matrix[2][2];
02471       rkInverse.m_Matrix[1][1] = m_Matrix[0][0]*m_Matrix[2][2] - m_Matrix[0][2]*m_Matrix[2][0];
02472       rkInverse.m_Matrix[1][2] = m_Matrix[0][2]*m_Matrix[1][0] - m_Matrix[0][0]*m_Matrix[1][2];
02473       rkInverse.m_Matrix[2][0] = m_Matrix[1][0]*m_Matrix[2][1] - m_Matrix[1][1]*m_Matrix[2][0];
02474       rkInverse.m_Matrix[2][1] = m_Matrix[0][1]*m_Matrix[2][0] - m_Matrix[0][0]*m_Matrix[2][1];
02475       rkInverse.m_Matrix[2][2] = m_Matrix[0][0]*m_Matrix[1][1] - m_Matrix[0][1]*m_Matrix[1][0];
02476 
02477       kt_double fDet = m_Matrix[0][0]*rkInverse.m_Matrix[0][0] +
02478                        m_Matrix[0][1]*rkInverse.m_Matrix[1][0] +
02479                        m_Matrix[0][2]*rkInverse.m_Matrix[2][0];
02480 
02481       if (fabs(fDet) <= fTolerance)
02482       {
02483         return false;
02484       }
02485 
02486       kt_double fInvDet = 1.0/fDet;
02487       for (size_t row = 0; row < 3; row++)
02488       {
02489         for (size_t col = 0; col < 3; col++)
02490         {
02491           rkInverse.m_Matrix[row][col] *= fInvDet;
02492         }
02493       }
02494 
02495       return true;
02496     }
02497 
02502     inline std::string ToString() const
02503     {
02504       std::stringstream converter;
02505       converter.precision(std::numeric_limits<double>::digits10);
02506 
02507       for (int row = 0; row < 3; row++)
02508       {
02509         for (int col = 0; col < 3; col++)
02510         {
02511           converter << m_Matrix[row][col] << " ";
02512         }
02513       }
02514 
02515       return converter.str();
02516     }
02517 
02518   public:
02522     inline Matrix3& operator = (const Matrix3& rOther)
02523     {
02524       memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
02525       return *this;
02526     }
02527 
02534     inline kt_double& operator()(kt_int32u row, kt_int32u column)
02535     {
02536       return m_Matrix[row][column];
02537     }
02538 
02545     inline kt_double operator()(kt_int32u row, kt_int32u column) const
02546     {
02547       return m_Matrix[row][column];
02548     }
02549 
02555     Matrix3 operator * (const Matrix3& rOther) const
02556     {
02557       Matrix3 product;
02558 
02559       for (size_t row = 0; row < 3; row++)
02560       {
02561         for (size_t col = 0; col < 3; col++)
02562         {
02563           product.m_Matrix[row][col] = m_Matrix[row][0]*rOther.m_Matrix[0][col] +
02564                                        m_Matrix[row][1]*rOther.m_Matrix[1][col] +
02565                                        m_Matrix[row][2]*rOther.m_Matrix[2][col];
02566         }
02567       }
02568 
02569       return product;
02570     }
02571 
02577     inline Pose2 operator * (const Pose2& rPose2) const
02578     {
02579       Pose2 pose2;
02580 
02581       pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] *
02582                  rPose2.GetY() + m_Matrix[0][2] * rPose2.GetHeading());
02583       pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] *
02584                  rPose2.GetY() + m_Matrix[1][2] * rPose2.GetHeading());
02585       pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() + m_Matrix[2][1] *
02586                        rPose2.GetY() + m_Matrix[2][2] * rPose2.GetHeading());
02587 
02588       return pose2;
02589     }
02590 
02595     inline void operator += (const Matrix3& rkMatrix)
02596     {
02597       for (kt_int32u row = 0; row < 3; row++)
02598       {
02599         for (kt_int32u col = 0; col < 3; col++)
02600         {
02601           m_Matrix[row][col] += rkMatrix.m_Matrix[row][col];
02602         }
02603       }
02604     }
02605 
02611     friend inline std::ostream& operator << (std::ostream& rStream, const Matrix3& rMatrix)
02612     {
02613       rStream << rMatrix.ToString();
02614       return rStream;
02615     }
02616 
02617   private:
02618     kt_double m_Matrix[3][3];
02619   };  // Matrix3
02620 
02624 
02628   class Matrix
02629   {
02630   public:
02634     Matrix(kt_int32u rows, kt_int32u columns)
02635       : m_Rows(rows)
02636       , m_Columns(columns)
02637       , m_pData(NULL)
02638     {
02639       Allocate();
02640 
02641       Clear();
02642     }
02643 
02647     virtual ~Matrix()
02648     {
02649       delete [] m_pData;
02650     }
02651 
02652   public:
02656     void Clear()
02657     {
02658       if (m_pData != NULL)
02659       {
02660         memset(m_pData, 0, sizeof(kt_double) * m_Rows * m_Columns);
02661       }
02662     }
02663 
02668     inline kt_int32u GetRows() const
02669     {
02670       return m_Rows;
02671     }
02672 
02677     inline kt_int32u GetColumns() const
02678     {
02679       return m_Columns;
02680     }
02681 
02688     inline kt_double& operator()(kt_int32u row, kt_int32u column)
02689     {
02690       RangeCheck(row, column);
02691 
02692       return m_pData[row + column * m_Rows];
02693     }
02694 
02701     inline const kt_double& operator()(kt_int32u row, kt_int32u column) const
02702     {
02703       RangeCheck(row, column);
02704 
02705       return m_pData[row + column * m_Rows];
02706     }
02707 
02708   private:
02712     void Allocate()
02713     {
02714       try
02715       {
02716         if (m_pData != NULL)
02717         {
02718           delete[] m_pData;
02719         }
02720 
02721         m_pData = new kt_double[m_Rows * m_Columns];
02722       }
02723       catch (const std::bad_alloc& ex)
02724       {
02725         throw Exception("Matrix allocation error");
02726       }
02727 
02728       if (m_pData == NULL)
02729       {
02730         throw Exception("Matrix allocation error");
02731       }
02732     }
02733 
02739     inline void RangeCheck(kt_int32u row, kt_int32u column) const
02740     {
02741       if (math::IsUpTo(row, m_Rows) == false)
02742       {
02743         throw Exception("Matrix - RangeCheck ERROR!!!!");
02744       }
02745 
02746       if (math::IsUpTo(column, m_Columns) == false)
02747       {
02748         throw Exception("Matrix - RangeCheck ERROR!!!!");
02749       }
02750     }
02751 
02752   private:
02753     kt_int32u m_Rows;
02754     kt_int32u m_Columns;
02755 
02756     kt_double* m_pData;
02757   };  // Matrix
02758 
02762 
02766   class BoundingBox2
02767   {
02768   public:
02769     /*
02770      * Default constructor
02771      */
02772     BoundingBox2()
02773       : m_Minimum(999999999999999999.99999, 999999999999999999.99999)
02774       , m_Maximum(-999999999999999999.99999, -999999999999999999.99999)
02775     {
02776     }
02777 
02778   public:
02782     inline const Vector2<kt_double>& GetMinimum() const
02783     {
02784       return m_Minimum;
02785     }
02786 
02790     inline void SetMinimum(const Vector2<kt_double>& mMinimum)
02791     {
02792       m_Minimum = mMinimum;
02793     }
02794 
02798     inline const Vector2<kt_double>& GetMaximum() const
02799     {
02800       return m_Maximum;
02801     }
02802 
02806     inline void SetMaximum(const Vector2<kt_double>& rMaximum)
02807     {
02808       m_Maximum = rMaximum;
02809     }
02810 
02814     inline Size2<kt_double> GetSize() const
02815     {
02816       Vector2<kt_double> size = m_Maximum - m_Minimum;
02817 
02818       return Size2<kt_double>(size.GetX(), size.GetY());
02819     }
02820 
02824     inline void Add(const Vector2<kt_double>& rPoint)
02825     {
02826       m_Minimum.MakeFloor(rPoint);
02827       m_Maximum.MakeCeil(rPoint);
02828     }
02829 
02833     inline void Add(const BoundingBox2& rBoundingBox)
02834     {
02835       Add(rBoundingBox.GetMinimum());
02836       Add(rBoundingBox.GetMaximum());
02837     }
02838 
02844     inline kt_bool IsInBounds(const Vector2<kt_double>& rPoint) const
02845     {
02846       return (math::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) &&
02847               math::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY()));
02848     }
02849 
02850   private:
02851     Vector2<kt_double> m_Minimum;
02852     Vector2<kt_double> m_Maximum;
02853   };  // BoundingBox2
02854 
02858 
02862   class Transform
02863   {
02864   public:
02869     Transform(const Pose2& rPose)
02870     {
02871       SetTransform(Pose2(), rPose);
02872     }
02873 
02879     Transform(const Pose2& rPose1, const Pose2& rPose2)
02880     {
02881       SetTransform(rPose1, rPose2);
02882     }
02883 
02884   public:
02890     inline Pose2 TransformPose(const Pose2& rSourcePose)
02891     {
02892       Pose2 newPosition = m_Transform + m_Rotation * rSourcePose;
02893       kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() + m_Transform.GetHeading());
02894 
02895       return Pose2(newPosition.GetPosition(), angle);
02896     }
02897 
02903     inline Pose2 InverseTransformPose(const Pose2& rSourcePose)
02904     {
02905       Pose2 newPosition = m_InverseRotation * (rSourcePose - m_Transform);
02906       kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() - m_Transform.GetHeading());
02907 
02908       // components of transform
02909       return Pose2(newPosition.GetPosition(), angle);
02910     }
02911 
02912   private:
02918     void SetTransform(const Pose2& rPose1, const Pose2& rPose2)
02919     {
02920       if (rPose1 == rPose2)
02921       {
02922         m_Rotation.SetToIdentity();
02923         m_InverseRotation.SetToIdentity();
02924         m_Transform = Pose2();
02925         return;
02926       }
02927 
02928       // heading transformation
02929       m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading());
02930       m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading());
02931 
02932       // position transformation
02933       Pose2 newPosition;
02934       if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0)
02935       {
02936         newPosition = rPose2 - m_Rotation * rPose1;
02937       }
02938       else
02939       {
02940         newPosition = rPose2;
02941       }
02942 
02943       m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading());
02944     }
02945 
02946   private:
02947     // pose transformation
02948     Pose2 m_Transform;
02949 
02950     Matrix3 m_Rotation;
02951     Matrix3 m_InverseRotation;
02952   };  // Transform
02953 
02957 
02961   typedef enum
02962   {
02963     LaserRangeFinder_Custom = 0,
02964 
02965     LaserRangeFinder_Sick_LMS100 = 1,
02966     LaserRangeFinder_Sick_LMS200 = 2,
02967     LaserRangeFinder_Sick_LMS291 = 3,
02968 
02969     LaserRangeFinder_Hokuyo_UTM_30LX = 4,
02970     LaserRangeFinder_Hokuyo_URG_04LX = 5
02971   } LaserRangeFinderType;
02972 
02976 
02980   class AbstractParameter
02981   {
02982   public:
02988     AbstractParameter(const std::string& rName, ParameterManager* pParameterManger = NULL)
02989       : m_Name(rName)
02990     {
02991       // if parameter manager is provided add myself to it!
02992       if (pParameterManger != NULL)
02993       {
02994         pParameterManger->Add(this);
02995       }
02996     }
02997 
03004     AbstractParameter(const std::string& rName,
03005                       const std::string& rDescription,
03006                       ParameterManager* pParameterManger = NULL)
03007       : m_Name(rName)
03008       , m_Description(rDescription)
03009     {
03010       // if parameter manager is provided add myself to it!
03011       if (pParameterManger != NULL)
03012       {
03013         pParameterManger->Add(this);
03014       }
03015     }
03016 
03020     virtual ~AbstractParameter()
03021     {
03022     }
03023 
03024   public:
03029     inline const std::string& GetName() const
03030     {
03031       return m_Name;
03032     }
03033 
03038     inline const std::string& GetDescription() const
03039     {
03040       return m_Description;
03041     }
03042 
03047     virtual const std::string GetValueAsString() const = 0;
03048 
03053     virtual void SetValueFromString(const std::string& rStringValue) = 0;
03054 
03059     virtual AbstractParameter* Clone() = 0;
03060 
03061   public:
03067     friend std::ostream& operator << (std::ostream& rStream, const AbstractParameter& rParameter)
03068     {
03069       rStream.precision(6);
03070       rStream.flags(std::ios::fixed);
03071 
03072       rStream << rParameter.GetName() << " = " << rParameter.GetValueAsString();
03073       return rStream;
03074     }
03075 
03076   private:
03077     std::string m_Name;
03078     std::string m_Description;
03079   };  // AbstractParameter
03080 
03084 
03088   template<typename T>
03089   class Parameter : public AbstractParameter
03090   {
03091   public:
03098     Parameter(const std::string& rName, T value, ParameterManager* pParameterManger = NULL)
03099       : AbstractParameter(rName, pParameterManger)
03100       , m_Value(value)
03101     {
03102     }
03103 
03111     Parameter(const std::string& rName,
03112               const std::string& rDescription,
03113               T value,
03114               ParameterManager* pParameterManger = NULL)
03115       : AbstractParameter(rName, rDescription, pParameterManger)
03116       , m_Value(value)
03117     {
03118     }
03119 
03123     virtual ~Parameter()
03124     {
03125     }
03126 
03127   public:
03132     inline const T& GetValue() const
03133     {
03134       return m_Value;
03135     }
03136 
03141     inline void SetValue(const T& rValue)
03142     {
03143       m_Value = rValue;
03144     }
03145 
03150     virtual const std::string GetValueAsString() const
03151     {
03152       std::stringstream converter;
03153       converter << m_Value;
03154       return converter.str();
03155     }
03156 
03161     virtual void SetValueFromString(const std::string& rStringValue)
03162     {
03163       std::stringstream converter;
03164       converter.str(rStringValue);
03165       converter >> m_Value;
03166     }
03167 
03172     virtual Parameter* Clone()
03173     {
03174       return new Parameter(GetName(), GetDescription(), GetValue());
03175     }
03176 
03177   public:
03181     Parameter& operator = (const Parameter& rOther)
03182     {
03183       m_Value = rOther.m_Value;
03184 
03185       return *this;
03186     }
03187 
03191     T operator = (T value)
03192     {
03193       m_Value = value;
03194 
03195       return m_Value;
03196     }
03197 
03198   protected:
03202     T m_Value;
03203   };  // Parameter
03204 
03205   template<>
03206   inline void Parameter<kt_double>::SetValueFromString(const std::string& rStringValue)
03207   {
03208     int precision = std::numeric_limits<double>::digits10;
03209     std::stringstream converter;
03210     converter.precision(precision);
03211 
03212     converter.str(rStringValue);
03213 
03214     m_Value = 0.0;
03215     converter >> m_Value;
03216   }
03217 
03218   template<>
03219   inline const std::string Parameter<kt_double>::GetValueAsString() const
03220   {
03221     std::stringstream converter;
03222     converter.precision(std::numeric_limits<double>::digits10);
03223     converter << m_Value;
03224     return converter.str();
03225   }
03226 
03227   template<>
03228   inline void Parameter<kt_bool>::SetValueFromString(const std::string& rStringValue)
03229   {
03230     if (rStringValue == "true" || rStringValue == "TRUE")
03231     {
03232       m_Value = true;
03233     }
03234     else
03235     {
03236       m_Value = false;
03237     }
03238   }
03239 
03240   template<>
03241   inline const std::string Parameter<kt_bool>::GetValueAsString() const
03242   {
03243     if (m_Value == true)
03244     {
03245       return "true";
03246     }
03247 
03248     return "false";
03249   }
03250 
03254 
03258   class ParameterEnum : public Parameter<kt_int32s>
03259   {
03260     typedef std::map<std::string, kt_int32s> EnumMap;
03261 
03262   public:
03269     ParameterEnum(const std::string& rName, kt_int32s value, ParameterManager* pParameterManger = NULL)
03270       : Parameter<kt_int32s>(rName, value, pParameterManger)
03271     {
03272     }
03273 
03277     virtual ~ParameterEnum()
03278     {
03279     }
03280 
03281   public:
03286     virtual Parameter<kt_int32s>* Clone()
03287     {
03288       ParameterEnum* pEnum = new ParameterEnum(GetName(), GetValue());
03289 
03290       pEnum->m_EnumDefines = m_EnumDefines;
03291 
03292       return pEnum;
03293     }
03294 
03299     virtual void SetValueFromString(const std::string& rStringValue)
03300     {
03301       if (m_EnumDefines.find(rStringValue) != m_EnumDefines.end())
03302       {
03303         m_Value = m_EnumDefines[rStringValue];
03304       }
03305       else
03306       {
03307         std::string validValues;
03308 
03309         const_forEach(EnumMap, &m_EnumDefines)
03310         {
03311           validValues += iter->first + ", ";
03312         }
03313 
03314         throw Exception("Unable to set enum: " + rStringValue + ". Valid values are: " + validValues);
03315       }
03316     }
03317 
03322     virtual const std::string GetValueAsString() const
03323     {
03324       const_forEach(EnumMap, &m_EnumDefines)
03325       {
03326         if (iter->second == m_Value)
03327         {
03328           return iter->first;
03329         }
03330       }
03331 
03332       throw Exception("Unable to lookup enum");
03333     }
03334 
03340     void DefineEnumValue(kt_int32s value, const std::string& rName)
03341     {
03342       if (m_EnumDefines.find(rName) == m_EnumDefines.end())
03343       {
03344         m_EnumDefines[rName] = value;
03345       }
03346       else
03347       {
03348         std::cerr << "Overriding enum value: " << m_EnumDefines[rName] << " with " << value << std::endl;
03349 
03350         m_EnumDefines[rName] = value;
03351 
03352         assert(false);
03353       }
03354     }
03355 
03356   public:
03360     ParameterEnum& operator = (const ParameterEnum& rOther)
03361     {
03362       SetValue(rOther.GetValue());
03363 
03364       return *this;
03365     }
03366 
03370     kt_int32s operator = (kt_int32s value)
03371     {
03372       SetValue(value);
03373 
03374       return m_Value;
03375     }
03376 
03377   private:
03378     EnumMap m_EnumDefines;
03379   };  // ParameterEnum
03380 
03384 
03388   class Parameters : public Object
03389   {
03390   public:
03391     // @cond EXCLUDE
03392     KARTO_Object(Parameters)
03393     // @endcond
03394 
03395   public:
03400     Parameters(const std::string& rName)
03401       : Object(rName)
03402     {
03403     }
03404 
03408     virtual ~Parameters()
03409     {
03410     }
03411 
03412   private:
03413     Parameters(const Parameters&);
03414     const Parameters& operator=(const Parameters&);
03415   };  // Parameters
03416 
03420 
03421   class SensorData;
03422 
03426   class KARTO_EXPORT Sensor : public Object
03427   {
03428   public:
03429     // @cond EXCLUDE
03430     KARTO_Object(Sensor)
03431     // @endcond
03432 
03433   protected:
03438     Sensor(const Name& rName);
03439 
03440   public:
03444     virtual ~Sensor();
03445 
03446   public:
03451     inline const Pose2& GetOffsetPose() const
03452     {
03453       return m_pOffsetPose->GetValue();
03454     }
03455 
03460     inline void SetOffsetPose(const Pose2& rPose)
03461     {
03462       m_pOffsetPose->SetValue(rPose);
03463     }
03464 
03469     virtual kt_bool Validate() = 0;
03470 
03476     virtual kt_bool Validate(SensorData* pSensorData) = 0;
03477 
03478   private:
03482     Sensor(const Sensor&);
03483 
03487     const Sensor& operator=(const Sensor&);
03488 
03489   private:
03493     Parameter<Pose2>* m_pOffsetPose;
03494   };  // Sensor
03495 
03499   typedef std::vector<Sensor*> SensorVector;
03500 
03504 
03508   typedef std::map<Name, Sensor*> SensorManagerMap;
03509 
03513   class KARTO_EXPORT SensorManager
03514   {
03515   public:
03519     SensorManager()
03520     {
03521     }
03522 
03526     virtual ~SensorManager()
03527     {
03528     }
03529 
03530   public:
03534     static SensorManager* GetInstance();
03535 
03536   public:
03544     void RegisterSensor(Sensor* pSensor, kt_bool override = false)
03545     {
03546       Validate(pSensor);
03547 
03548       if ((m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) && !override)
03549       {
03550         throw Exception("Cannot register sensor: already registered: [" +
03551                         pSensor->GetName().ToString() +
03552                         "] (Consider setting 'override' to true)");
03553       }
03554 
03555       std::cout << "Registering sensor: [" << pSensor->GetName().ToString() << "]" << std::endl;
03556 
03557       m_Sensors[pSensor->GetName()] = pSensor;
03558     }
03559 
03564     void UnregisterSensor(Sensor* pSensor)
03565     {
03566       Validate(pSensor);
03567 
03568       if (m_Sensors.find(pSensor->GetName()) != m_Sensors.end())
03569       {
03570         std::cout << "Unregistering sensor: " << pSensor->GetName().ToString() << std::endl;
03571 
03572         m_Sensors.erase(pSensor->GetName());
03573       }
03574       else
03575       {
03576         throw Exception("Cannot unregister sensor: not registered: [" + pSensor->GetName().ToString() + "]");
03577       }
03578     }
03579 
03585     Sensor* GetSensorByName(const Name& rName)
03586     {
03587       if (m_Sensors.find(rName) != m_Sensors.end())
03588       {
03589         return m_Sensors[rName];
03590       }
03591 
03592       throw Exception("Sensor not registered: [" + rName.ToString() + "] (Did you add the sensor to the Dataset?)");
03593     }
03594 
03600     template<class T>
03601     T* GetSensorByName(const Name& rName)
03602     {
03603       Sensor* pSensor = GetSensorByName(rName);
03604 
03605       return dynamic_cast<T*>(pSensor);
03606     }
03607 
03612     SensorVector GetAllSensors()
03613     {
03614       SensorVector sensors;
03615 
03616       forEach(SensorManagerMap, &m_Sensors)
03617       {
03618         sensors.push_back(iter->second);
03619       }
03620 
03621       return sensors;
03622     }
03623 
03624   protected:
03629     static void Validate(Sensor* pSensor)
03630     {
03631       if (pSensor == NULL)
03632       {
03633         throw Exception("Invalid sensor:  NULL");
03634       }
03635       else if (pSensor->GetName().ToString() == "")
03636       {
03637         throw Exception("Invalid sensor:  nameless");
03638       }
03639     }
03640 
03641   protected:
03645     SensorManagerMap m_Sensors;
03646   };
03647 
03651 
03658   class Drive : public Sensor
03659   {
03660   public:
03661     // @cond EXCLUDE
03662     KARTO_Object(Drive)
03663     // @endcond
03664 
03665   public:
03669     Drive(const std::string& rName)
03670       : Sensor(rName)
03671     {
03672     }
03673 
03677     virtual ~Drive()
03678     {
03679     }
03680 
03681   public:
03682     virtual kt_bool Validate()
03683     {
03684       return true;
03685     }
03686 
03687     virtual kt_bool Validate(SensorData* pSensorData)
03688     {
03689       if (pSensorData == NULL)
03690       {
03691         return false;
03692       }
03693 
03694       return true;
03695     }
03696 
03697   private:
03698     Drive(const Drive&);
03699     const Drive& operator=(const Drive&);
03700   };  // class Drive
03701 
03705 
03706   class LocalizedRangeScan;
03707   class CoordinateConverter;
03708 
03721   class KARTO_EXPORT LaserRangeFinder : public Sensor
03722   {
03723   public:
03724     // @cond EXCLUDE
03725     KARTO_Object(LaserRangeFinder)
03726     // @endcond
03727 
03728   public:
03732     virtual ~LaserRangeFinder()
03733     {
03734     }
03735 
03736   public:
03741     inline kt_double GetMinimumRange() const
03742     {
03743       return m_pMinimumRange->GetValue();
03744     }
03745 
03750     inline void SetMinimumRange(kt_double minimumRange)
03751     {
03752       m_pMinimumRange->SetValue(minimumRange);
03753 
03754       SetRangeThreshold(GetRangeThreshold());
03755     }
03756 
03761     inline kt_double GetMaximumRange() const
03762     {
03763       return m_pMaximumRange->GetValue();
03764     }
03765 
03770     inline void SetMaximumRange(kt_double maximumRange)
03771     {
03772       m_pMaximumRange->SetValue(maximumRange);
03773 
03774       SetRangeThreshold(GetRangeThreshold());
03775     }
03776 
03781     inline kt_double GetRangeThreshold() const
03782     {
03783       return m_pRangeThreshold->GetValue();
03784     }
03785 
03790     inline void SetRangeThreshold(kt_double rangeThreshold)
03791     {
03792       // make sure rangeThreshold is within laser range finder range
03793       m_pRangeThreshold->SetValue(math::Clip(rangeThreshold, GetMinimumRange(), GetMaximumRange()));
03794 
03795       if (math::DoubleEqual(GetRangeThreshold(), rangeThreshold) == false)
03796       {
03797         std::cout << "Info: clipped range threshold to be within minimum and maximum range!" << std::endl;
03798       }
03799     }
03800 
03805     inline kt_double GetMinimumAngle() const
03806     {
03807       return m_pMinimumAngle->GetValue();
03808     }
03809 
03814     inline void SetMinimumAngle(kt_double minimumAngle)
03815     {
03816       m_pMinimumAngle->SetValue(minimumAngle);
03817 
03818       Update();
03819     }
03820 
03825     inline kt_double GetMaximumAngle() const
03826     {
03827       return m_pMaximumAngle->GetValue();
03828     }
03829 
03834     inline void SetMaximumAngle(kt_double maximumAngle)
03835     {
03836       m_pMaximumAngle->SetValue(maximumAngle);
03837 
03838       Update();
03839     }
03840 
03845     inline kt_double GetAngularResolution() const
03846     {
03847       return m_pAngularResolution->GetValue();
03848     }
03849 
03854     inline void SetAngularResolution(kt_double angularResolution)
03855     {
03856       if (m_pType->GetValue() == LaserRangeFinder_Custom)
03857       {
03858         m_pAngularResolution->SetValue(angularResolution);
03859       }
03860       else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS100)
03861       {
03862         if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
03863         {
03864           m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
03865         }
03866         else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
03867         {
03868           m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
03869         }
03870         else
03871         {
03872           std::stringstream stream;
03873           stream << "Invalid resolution for Sick LMS100:  ";
03874           stream << angularResolution;
03875           throw Exception(stream.str());
03876         }
03877       }
03878       else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS200 ||
03879                m_pType->GetValue() == LaserRangeFinder_Sick_LMS291)
03880       {
03881         if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
03882         {
03883           m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
03884         }
03885         else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
03886         {
03887           m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
03888         }
03889         else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(1.00)))
03890         {
03891           m_pAngularResolution->SetValue(math::DegreesToRadians(1.00));
03892         }
03893         else
03894         {
03895           std::stringstream stream;
03896           stream << "Invalid resolution for Sick LMS291:  ";
03897           stream << angularResolution;
03898           throw Exception(stream.str());
03899         }
03900       }
03901       else
03902       {
03903         throw Exception("Can't set angular resolution, please create a LaserRangeFinder of type Custom");
03904       }
03905 
03906       Update();
03907     }
03908 
03912     inline kt_int32s GetType()
03913     {
03914       return m_pType->GetValue();
03915     }
03916 
03921     inline kt_int32u GetNumberOfRangeReadings() const
03922     {
03923       return m_NumberOfRangeReadings;
03924     }
03925 
03926     virtual kt_bool Validate()
03927     {
03928       Update();
03929 
03930       if (math::InRange(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange()) == false)
03931       {
03932         std::cout << "Please set range threshold to a value between ["
03933                   << GetMinimumRange() << ";" << GetMaximumRange() << "]" << std::endl;
03934         return false;
03935       }
03936 
03937       return true;
03938     }
03939 
03940     virtual kt_bool Validate(SensorData* pSensorData);
03941 
03949     const PointVectorDouble GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan,
03950                                             CoordinateConverter* pCoordinateConverter,
03951                                             kt_bool ignoreThresholdPoints = true,
03952                                             kt_bool flipY = false) const;
03953 
03954   public:
03961     static LaserRangeFinder* CreateLaserRangeFinder(LaserRangeFinderType type, const Name& rName)
03962     {
03963       LaserRangeFinder* pLrf = NULL;
03964 
03965       switch (type)
03966       {
03967         // see http://www.hizook.com/files/publications/SICK_LMS100.pdf
03968         // set range threshold to 18m
03969         case LaserRangeFinder_Sick_LMS100:
03970         {
03971           pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 100"));
03972 
03973           // Sensing range is 18 meters (at 10% reflectivity, max range of 20 meters), with an error of about 20mm
03974           pLrf->m_pMinimumRange->SetValue(0.0);
03975           pLrf->m_pMaximumRange->SetValue(20.0);
03976 
03977           // 270 degree range, 50 Hz
03978           pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135));
03979           pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135));
03980 
03981           // 0.25 degree angular resolution
03982           pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
03983 
03984           pLrf->m_NumberOfRangeReadings = 1081;
03985         }
03986         break;
03987 
03988         // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
03989         // set range threshold to 10m
03990         case LaserRangeFinder_Sick_LMS200:
03991         {
03992           pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 200"));
03993 
03994           // Sensing range is 80 meters
03995           pLrf->m_pMinimumRange->SetValue(0.0);
03996           pLrf->m_pMaximumRange->SetValue(80.0);
03997 
03998           // 180 degree range, 75 Hz
03999           pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
04000           pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
04001 
04002           // 0.5 degree angular resolution
04003           pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5));
04004 
04005           pLrf->m_NumberOfRangeReadings = 361;
04006         }
04007         break;
04008 
04009         // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
04010         // set range threshold to 30m
04011         case LaserRangeFinder_Sick_LMS291:
04012         {
04013           pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 291"));
04014 
04015           // Sensing range is 80 meters
04016           pLrf->m_pMinimumRange->SetValue(0.0);
04017           pLrf->m_pMaximumRange->SetValue(80.0);
04018 
04019           // 180 degree range, 75 Hz
04020           pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
04021           pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
04022 
04023           // 0.5 degree angular resolution
04024           pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.5));
04025 
04026           pLrf->m_NumberOfRangeReadings = 361;
04027         }
04028         break;
04029 
04030         // see http://www.hizook.com/files/publications/Hokuyo_UTM_LaserRangeFinder_LIDAR.pdf
04031         // set range threshold to 30m
04032         case LaserRangeFinder_Hokuyo_UTM_30LX:
04033         {
04034           pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo UTM-30LX"));
04035 
04036           // Sensing range is 30 meters
04037           pLrf->m_pMinimumRange->SetValue(0.1);
04038           pLrf->m_pMaximumRange->SetValue(30.0);
04039 
04040           // 270 degree range, 40 Hz
04041           pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-135));
04042           pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(135));
04043 
04044           // 0.25 degree angular resolution
04045           pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
04046 
04047           pLrf->m_NumberOfRangeReadings = 1081;
04048         }
04049         break;
04050 
04051         // see http://www.hizook.com/files/publications/HokuyoURG_Datasheet.pdf
04052         // set range threshold to 3.5m
04053         case LaserRangeFinder_Hokuyo_URG_04LX:
04054         {
04055           pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo URG-04LX"));
04056 
04057           // Sensing range is 4 meters. It has detection problems when
04058           // scanning absorptive surfaces (such as black trimming).
04059           pLrf->m_pMinimumRange->SetValue(0.02);
04060           pLrf->m_pMaximumRange->SetValue(4.0);
04061 
04062           // 240 degree range, 10 Hz
04063           pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-120));
04064           pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(120));
04065 
04066           // 0.352 degree angular resolution
04067           pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(0.352));
04068 
04069           pLrf->m_NumberOfRangeReadings = 751;
04070         }
04071         break;
04072 
04073         case LaserRangeFinder_Custom:
04074         {
04075           pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("User-Defined LaserRangeFinder"));
04076 
04077           // Sensing range is 80 meters.
04078           pLrf->m_pMinimumRange->SetValue(0.0);
04079           pLrf->m_pMaximumRange->SetValue(80.0);
04080 
04081           // 180 degree range
04082           pLrf->m_pMinimumAngle->SetValue(math::DegreesToRadians(-90));
04083           pLrf->m_pMaximumAngle->SetValue(math::DegreesToRadians(90));
04084 
04085           // 1.0 degree angular resolution
04086           pLrf->m_pAngularResolution->SetValue(math::DegreesToRadians(1.0));
04087 
04088           pLrf->m_NumberOfRangeReadings = 181;
04089         }
04090         break;
04091       }
04092 
04093       if (pLrf != NULL)
04094       {
04095         pLrf->m_pType->SetValue(type);
04096 
04097         Pose2 defaultOffset;
04098         pLrf->SetOffsetPose(defaultOffset);
04099       }
04100 
04101       return pLrf;
04102     }
04103 
04104   private:
04108     LaserRangeFinder(const Name& rName)
04109       : Sensor(rName)
04110       , m_NumberOfRangeReadings(0)
04111     {
04112       m_pMinimumRange = new Parameter<kt_double>("MinimumRange", 0.0, GetParameterManager());
04113       m_pMaximumRange = new Parameter<kt_double>("MaximumRange", 80.0, GetParameterManager());
04114 
04115       m_pMinimumAngle = new Parameter<kt_double>("MinimumAngle", -KT_PI_2, GetParameterManager());
04116       m_pMaximumAngle = new Parameter<kt_double>("MaximumAngle", KT_PI_2, GetParameterManager());
04117 
04118       m_pAngularResolution = new Parameter<kt_double>("AngularResolution",
04119                                                       math::DegreesToRadians(1),
04120                                                       GetParameterManager());
04121 
04122       m_pRangeThreshold = new Parameter<kt_double>("RangeThreshold", 12.0, GetParameterManager());
04123 
04124       m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager());
04125       m_pType->DefineEnumValue(LaserRangeFinder_Custom, "Custom");
04126       m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS100, "Sick_LMS100");
04127       m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS200, "Sick_LMS200");
04128       m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS291, "Sick_LMS291");
04129       m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_UTM_30LX, "Hokuyo_UTM_30LX");
04130       m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_URG_04LX, "Hokuyo_URG_04LX");
04131     }
04132 
04137     void Update()
04138     {
04139       m_NumberOfRangeReadings = static_cast<kt_int32u>(math::Round((GetMaximumAngle() -
04140                                                                     GetMinimumAngle())
04141                                                                     / GetAngularResolution()) + 1);
04142     }
04143 
04144   private:
04145     LaserRangeFinder(const LaserRangeFinder&);
04146     const LaserRangeFinder& operator=(const LaserRangeFinder&);
04147 
04148   private:
04149     // sensor m_Parameters
04150     Parameter<kt_double>* m_pMinimumAngle;
04151     Parameter<kt_double>* m_pMaximumAngle;
04152 
04153     Parameter<kt_double>* m_pAngularResolution;
04154 
04155     Parameter<kt_double>* m_pMinimumRange;
04156     Parameter<kt_double>* m_pMaximumRange;
04157 
04158     Parameter<kt_double>* m_pRangeThreshold;
04159 
04160     ParameterEnum* m_pType;
04161 
04162     kt_int32u m_NumberOfRangeReadings;
04163 
04164     // static std::string LaserRangeFinderTypeNames[6];
04165   };  // LaserRangeFinder
04166 
04170 
04174   typedef enum
04175   {
04176     GridStates_Unknown = 0,
04177     GridStates_Occupied = 100,
04178     GridStates_Free = 255
04179   } GridStates;
04180 
04184 
04190   class CoordinateConverter
04191   {
04192   public:
04196     CoordinateConverter()
04197       : m_Scale(20.0)
04198     {
04199     }
04200 
04201   public:
04207     inline kt_double Transform(kt_double value)
04208     {
04209       return value * m_Scale;
04210     }
04211 
04218     inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double>& rWorld, kt_bool flipY = false) const
04219     {
04220       kt_double gridX = (rWorld.GetX() - m_Offset.GetX()) * m_Scale;
04221       kt_double gridY = 0.0;
04222 
04223       if (flipY == false)
04224       {
04225         gridY = (rWorld.GetY() - m_Offset.GetY()) * m_Scale;
04226       }
04227       else
04228       {
04229         gridY = (m_Size.GetHeight() / m_Scale - rWorld.GetY() + m_Offset.GetY()) * m_Scale;
04230       }
04231 
04232       return Vector2<kt_int32s>(static_cast<kt_int32s>(math::Round(gridX)), static_cast<kt_int32s>(math::Round(gridY)));
04233     }
04234 
04241     inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s>& rGrid, kt_bool flipY = false) const
04242     {
04243       kt_double worldX = m_Offset.GetX() + rGrid.GetX() / m_Scale;
04244       kt_double worldY = 0.0;
04245 
04246       if (flipY == false)
04247       {
04248         worldY = m_Offset.GetY() + rGrid.GetY() / m_Scale;
04249       }
04250       else
04251       {
04252         worldY = m_Offset.GetY() + (m_Size.GetHeight() - rGrid.GetY()) / m_Scale;
04253       }
04254 
04255       return Vector2<kt_double>(worldX, worldY);
04256     }
04257 
04262     inline kt_double GetScale() const
04263     {
04264       return m_Scale;
04265     }
04266 
04271     inline void SetScale(kt_double scale)
04272     {
04273       m_Scale = scale;
04274     }
04275 
04280     inline const Vector2<kt_double>& GetOffset() const
04281     {
04282       return m_Offset;
04283     }
04284 
04289     inline void SetOffset(const Vector2<kt_double>& rOffset)
04290     {
04291       m_Offset = rOffset;
04292     }
04293 
04298     inline void SetSize(const Size2<kt_int32s>& rSize)
04299     {
04300       m_Size = rSize;
04301     }
04302 
04307     inline const Size2<kt_int32s>& GetSize() const
04308     {
04309       return m_Size;
04310     }
04311 
04316     inline kt_double GetResolution() const
04317     {
04318       return 1.0 / m_Scale;
04319     }
04320 
04325     inline void SetResolution(kt_double resolution)
04326     {
04327       m_Scale = 1.0 / resolution;
04328     }
04329 
04334     inline BoundingBox2 GetBoundingBox() const
04335     {
04336       BoundingBox2 box;
04337 
04338       kt_double minX = GetOffset().GetX();
04339       kt_double minY = GetOffset().GetY();
04340       kt_double maxX = minX + GetSize().GetWidth() * GetResolution();
04341       kt_double maxY = minY + GetSize().GetHeight() * GetResolution();
04342 
04343       box.SetMinimum(GetOffset());
04344       box.SetMaximum(Vector2<kt_double>(maxX, maxY));
04345       return box;
04346     }
04347 
04348   private:
04349     Size2<kt_int32s> m_Size;
04350     kt_double m_Scale;
04351 
04352     Vector2<kt_double> m_Offset;
04353   };  // CoordinateConverter
04354 
04358 
04362   template<typename T>
04363   class Grid
04364   {
04365   public:
04373     static Grid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
04374     {
04375       Grid* pGrid = new Grid(width, height);
04376 
04377       pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution);
04378 
04379       return pGrid;
04380     }
04381 
04385     virtual ~Grid()
04386     {
04387       delete [] m_pData;
04388       delete m_pCoordinateConverter;
04389     }
04390 
04391   public:
04395     void Clear()
04396     {
04397       memset(m_pData, 0, GetDataSize() * sizeof(T));
04398     }
04399 
04404     Grid* Clone()
04405     {
04406       Grid* pGrid = CreateGrid(GetWidth(), GetHeight(), GetResolution());
04407       pGrid->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
04408 
04409       memcpy(pGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
04410 
04411       return pGrid;
04412     }
04413 
04419     virtual void Resize(kt_int32s width, kt_int32s height)
04420     {
04421       m_Width = width;
04422       m_Height = height;
04423       m_WidthStep = math::AlignValue<kt_int32s>(width, 8);
04424 
04425       if (m_pData != NULL)
04426       {
04427         delete[] m_pData;
04428         m_pData = NULL;
04429       }
04430 
04431       try
04432       {
04433         m_pData = new T[GetDataSize()];
04434 
04435         if (m_pCoordinateConverter == NULL)
04436         {
04437           m_pCoordinateConverter = new CoordinateConverter();
04438         }
04439 
04440         m_pCoordinateConverter->SetSize(Size2<kt_int32s>(width, height));
04441       }
04442       catch(...)
04443       {
04444         m_pData = NULL;
04445 
04446         m_Width = 0;
04447         m_Height = 0;
04448         m_WidthStep = 0;
04449       }
04450 
04451       Clear();
04452     }
04453 
04458     inline kt_bool IsValidGridIndex(const Vector2<kt_int32s>& rGrid) const
04459     {
04460       return (math::IsUpTo(rGrid.GetX(), m_Width) && math::IsUpTo(rGrid.GetY(), m_Height));
04461     }
04462 
04469     virtual kt_int32s GridIndex(const Vector2<kt_int32s>& rGrid, kt_bool boundaryCheck = true) const
04470     {
04471       if (boundaryCheck == true)
04472       {
04473         if (IsValidGridIndex(rGrid) == false)
04474         {
04475           std::stringstream error;
04476           error << "Index " << rGrid << " out of range.  Index must be between [0; "
04477                 << m_Width << ") and [0; " << m_Height << ")";
04478           throw Exception(error.str());
04479         }
04480       }
04481 
04482       kt_int32s index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep);
04483 
04484       if (boundaryCheck == true)
04485       {
04486         assert(math::IsUpTo(index, GetDataSize()));
04487       }
04488 
04489       return index;
04490     }
04491 
04497     Vector2<kt_int32s> IndexToGrid(kt_int32s index) const
04498     {
04499       Vector2<kt_int32s> grid;
04500 
04501       grid.SetY(index / m_WidthStep);
04502       grid.SetX(index - grid.GetY() * m_WidthStep);
04503 
04504       return grid;
04505     }
04506 
04513     inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double>& rWorld, kt_bool flipY = false) const
04514     {
04515       return GetCoordinateConverter()->WorldToGrid(rWorld, flipY);
04516     }
04517 
04524     inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s>& rGrid, kt_bool flipY = false) const
04525     {
04526       return GetCoordinateConverter()->GridToWorld(rGrid, flipY);
04527     }
04528 
04534     T* GetDataPointer(const Vector2<kt_int32s>& rGrid)
04535     {
04536       kt_int32s index = GridIndex(rGrid, true);
04537       return m_pData + index;
04538     }
04539 
04545     T* GetDataPointer(const Vector2<kt_int32s>& rGrid) const
04546     {
04547       kt_int32s index = GridIndex(rGrid, true);
04548       return m_pData + index;
04549     }
04550 
04555     inline kt_int32s GetWidth() const
04556     {
04557       return m_Width;
04558     };
04559 
04564     inline kt_int32s GetHeight() const
04565     {
04566       return m_Height;
04567     };
04568 
04573     inline const Size2<kt_int32s> GetSize() const
04574     {
04575       return Size2<kt_int32s>(m_Width, m_Height);
04576     }
04577 
04582     inline kt_int32s GetWidthStep() const
04583     {
04584       return m_WidthStep;
04585     }
04586 
04591     inline T* GetDataPointer()
04592     {
04593       return m_pData;
04594     }
04595 
04600     inline T* GetDataPointer() const
04601     {
04602       return m_pData;
04603     }
04604 
04609     inline kt_int32s GetDataSize() const
04610     {
04611       return m_WidthStep * m_Height;
04612     }
04613 
04619     inline T GetValue(const Vector2<kt_int32s>& rGrid) const
04620     {
04621       kt_int32s index = GridIndex(rGrid);
04622       return m_pData[index];
04623     }
04624 
04629     inline CoordinateConverter* GetCoordinateConverter() const
04630     {
04631       return m_pCoordinateConverter;
04632     }
04633 
04638     inline kt_double GetResolution() const
04639     {
04640       return GetCoordinateConverter()->GetResolution();
04641     }
04642 
04647     inline BoundingBox2 GetBoundingBox() const
04648     {
04649       return GetCoordinateConverter()->GetBoundingBox();
04650     }
04651 
04661     void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor* f = NULL)
04662     {
04663       kt_bool steep = abs(y1 - y0) > abs(x1 - x0);
04664       if (steep)
04665       {
04666         std::swap(x0, y0);
04667         std::swap(x1, y1);
04668       }
04669       if (x0 > x1)
04670       {
04671         std::swap(x0, x1);
04672         std::swap(y0, y1);
04673       }
04674 
04675       kt_int32s deltaX = x1 - x0;
04676       kt_int32s deltaY = abs(y1 - y0);
04677       kt_int32s error = 0;
04678       kt_int32s ystep;
04679       kt_int32s y = y0;
04680 
04681       if (y0 < y1)
04682       {
04683         ystep = 1;
04684       }
04685       else
04686       {
04687         ystep = -1;
04688       }
04689 
04690       kt_int32s pointX;
04691       kt_int32s pointY;
04692       for (kt_int32s x = x0; x <= x1; x++)
04693       {
04694         if (steep)
04695         {
04696           pointX = y;
04697           pointY = x;
04698         }
04699         else
04700         {
04701           pointX = x;
04702           pointY = y;
04703         }
04704 
04705         error += deltaY;
04706 
04707         if (2 * error >= deltaX)
04708         {
04709           y += ystep;
04710           error -= deltaX;
04711         }
04712 
04713         Vector2<kt_int32s> gridIndex(pointX, pointY);
04714         if (IsValidGridIndex(gridIndex))
04715         {
04716           kt_int32s index = GridIndex(gridIndex, false);
04717           T* pGridPointer = GetDataPointer();
04718           pGridPointer[index]++;
04719 
04720           if (f != NULL)
04721           {
04722             (*f)(index);
04723           }
04724         }
04725       }
04726     }
04727 
04728   protected:
04734     Grid(kt_int32s width, kt_int32s height)
04735       : m_pData(NULL)
04736       , m_pCoordinateConverter(NULL)
04737     {
04738       Resize(width, height);
04739     }
04740 
04741   private:
04742     kt_int32s m_Width;       // width of grid
04743     kt_int32s m_Height;      // height of grid
04744     kt_int32s m_WidthStep;   // 8 bit aligned width of grid
04745     T* m_pData;              // grid data
04746 
04747     // coordinate converter to convert between world coordinates and grid coordinates
04748     CoordinateConverter* m_pCoordinateConverter;
04749   };  // Grid
04750 
04754 
04758   class CustomData : public Object
04759   {
04760   public:
04761     // @cond EXCLUDE
04762     KARTO_Object(CustomData)
04763     // @endcond
04764 
04765   public:
04769     CustomData()
04770       : Object()
04771     {
04772     }
04773 
04777     virtual ~CustomData()
04778     {
04779     }
04780 
04781   public:
04786     virtual const std::string Write() const = 0;
04787 
04792     virtual void Read(const std::string& rValue) = 0;
04793 
04794   private:
04795     CustomData(const CustomData&);
04796     const CustomData& operator=(const CustomData&);
04797   };
04798 
04802   typedef std::vector<CustomData*> CustomDataVector;
04803 
04807 
04811   class KARTO_EXPORT SensorData : public Object
04812   {
04813   public:
04814     // @cond EXCLUDE
04815     KARTO_Object(SensorData)
04816     // @endcond
04817 
04818   public:
04822     virtual ~SensorData();
04823 
04824   public:
04829     inline kt_int32s GetStateId() const
04830     {
04831       return m_StateId;
04832     }
04833 
04838     inline void SetStateId(kt_int32s stateId)
04839     {
04840       m_StateId = stateId;
04841     }
04842 
04847     inline kt_int32s GetUniqueId() const
04848     {
04849       return m_UniqueId;
04850     }
04851 
04856     inline void SetUniqueId(kt_int32u uniqueId)
04857     {
04858       m_UniqueId = uniqueId;
04859     }
04860 
04865     inline kt_double GetTime() const
04866     {
04867       return m_Time;
04868     }
04869 
04874     inline void SetTime(kt_double time)
04875     {
04876       m_Time = time;
04877     }
04878 
04883     inline const Name& GetSensorName() const
04884     {
04885       return m_SensorName;
04886     }
04887 
04892     inline void AddCustomData(CustomData* pCustomData)
04893     {
04894       m_CustomData.push_back(pCustomData);
04895     }
04896 
04901     inline const CustomDataVector& GetCustomData() const
04902     {
04903       return m_CustomData;
04904     }
04905 
04906   protected:
04910     SensorData(const Name& rSensorName);
04911 
04912   private:
04916     SensorData(const SensorData&);
04917 
04921     const SensorData& operator=(const SensorData&);
04922 
04923   private:
04927     kt_int32s m_StateId;
04928 
04932     kt_int32s m_UniqueId;
04933 
04937     Name m_SensorName;
04938 
04942     kt_double m_Time;
04943 
04944     CustomDataVector m_CustomData;
04945   };
04946 
04950 
04954   typedef std::vector<kt_double> RangeReadingsVector;
04955 
04959   class LaserRangeScan : public SensorData
04960   {
04961   public:
04962     // @cond EXCLUDE
04963     KARTO_Object(LaserRangeScan)
04964     // @endcond
04965 
04966   public:
04971     LaserRangeScan(const Name& rSensorName)
04972       : SensorData(rSensorName)
04973       , m_pRangeReadings(NULL)
04974       , m_NumberOfRangeReadings(0)
04975     {
04976     }
04977 
04983     LaserRangeScan(const Name& rSensorName, const RangeReadingsVector& rRangeReadings)
04984       : SensorData(rSensorName)
04985       , m_pRangeReadings(NULL)
04986       , m_NumberOfRangeReadings(0)
04987     {
04988       assert(rSensorName.ToString() != "");
04989 
04990       SetRangeReadings(rRangeReadings);
04991     }
04992 
04996     virtual ~LaserRangeScan()
04997     {
04998       delete [] m_pRangeReadings;
04999     }
05000 
05001   public:
05006     inline kt_double* GetRangeReadings() const
05007     {
05008       return m_pRangeReadings;
05009     }
05010 
05011     inline RangeReadingsVector GetRangeReadingsVector() const
05012     {
05013       return RangeReadingsVector(m_pRangeReadings, m_pRangeReadings + m_NumberOfRangeReadings);
05014     }
05015 
05020     inline void SetRangeReadings(const RangeReadingsVector& rRangeReadings)
05021     {
05022       // ignore for now! XXXMAE BUGUBUG 05/21/2010 << TODO(lucbettaieb): What the heck is this??
05023       // if (rRangeReadings.size() != GetNumberOfRangeReadings())
05024       // {
05025       //   std::stringstream error;
05026       //   error << "Given number of readings (" << rRangeReadings.size()
05027       //         << ") does not match expected number of range finder (" << GetNumberOfRangeReadings() << ")";
05028       //   throw Exception(error.str());
05029       // }
05030 
05031       if (!rRangeReadings.empty())
05032       {
05033         if (rRangeReadings.size() != m_NumberOfRangeReadings)
05034         {
05035           // delete old readings
05036           delete [] m_pRangeReadings;
05037 
05038           // store size of array!
05039           m_NumberOfRangeReadings = static_cast<kt_int32u>(rRangeReadings.size());
05040 
05041           // allocate range readings
05042           m_pRangeReadings = new kt_double[m_NumberOfRangeReadings];
05043         }
05044 
05045         // copy readings
05046         kt_int32u index = 0;
05047         const_forEach(RangeReadingsVector, &rRangeReadings)
05048         {
05049           m_pRangeReadings[index++] = *iter;
05050         }
05051       }
05052       else
05053       {
05054         delete [] m_pRangeReadings;
05055         m_pRangeReadings = NULL;
05056       }
05057     }
05058 
05063     inline LaserRangeFinder* GetLaserRangeFinder() const
05064     {
05065       return SensorManager::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorName());
05066     }
05067 
05072     inline kt_int32u GetNumberOfRangeReadings() const
05073     {
05074       return m_NumberOfRangeReadings;
05075     }
05076 
05077   private:
05078     LaserRangeScan(const LaserRangeScan&);
05079     const LaserRangeScan& operator=(const LaserRangeScan&);
05080 
05081   private:
05082     kt_double* m_pRangeReadings;
05083     kt_int32u m_NumberOfRangeReadings;
05084   };  // LaserRangeScan
05085 
05089 
05093   class DrivePose : public SensorData
05094   {
05095   public:
05096     // @cond EXCLUDE
05097     KARTO_Object(DrivePose)
05098     // @endcond
05099 
05100   public:
05105     DrivePose(const Name& rSensorName)
05106       : SensorData(rSensorName)
05107     {
05108     }
05109 
05113     virtual ~DrivePose()
05114     {
05115     }
05116 
05117   public:
05122     inline const Pose3& GetOdometricPose() const
05123     {
05124       return m_OdometricPose;
05125     }
05126 
05131     inline void SetOdometricPose(const Pose3& rPose)
05132     {
05133       m_OdometricPose = rPose;
05134     }
05135 
05136   private:
05137     DrivePose(const DrivePose&);
05138     const DrivePose& operator=(const DrivePose&);
05139 
05140   private:
05144     Pose3 m_OdometricPose;
05145   };  // class DrivePose
05146 
05150 
05157   class LocalizedRangeScan : public LaserRangeScan
05158   {
05159   public:
05160     // @cond EXCLUDE
05161     KARTO_Object(LocalizedRangeScan)
05162     // @endcond
05163 
05164   public:
05168     LocalizedRangeScan(const Name& rSensorName, const RangeReadingsVector& rReadings)
05169       : LaserRangeScan(rSensorName, rReadings)
05170       , m_IsDirty(true)
05171     {
05172     }
05173 
05177     virtual ~LocalizedRangeScan()
05178     {
05179     }
05180 
05181   private:
05182     mutable boost::shared_mutex m_Lock;
05183 
05184   public:
05189     inline const Pose2& GetOdometricPose() const
05190     {
05191       return m_OdometricPose;
05192     }
05193 
05198     inline void SetOdometricPose(const Pose2& rPose)
05199     {
05200       m_OdometricPose = rPose;
05201     }
05202 
05211     inline const Pose2& GetCorrectedPose() const
05212     {
05213       return m_CorrectedPose;
05214     }
05215 
05220     inline void SetCorrectedPose(const Pose2& rPose)
05221     {
05222       m_CorrectedPose = rPose;
05223 
05224       m_IsDirty = true;
05225     }
05226 
05230     inline const Pose2& GetBarycenterPose() const
05231     {
05232       boost::shared_lock<boost::shared_mutex> lock(m_Lock);
05233       if (m_IsDirty)
05234       {
05235         // throw away constness and do an update!
05236         lock.unlock();
05237         boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
05238         const_cast<LocalizedRangeScan*>(this)->Update();
05239       }
05240 
05241       return m_BarycenterPose;
05242     }
05243 
05249     inline Pose2 GetReferencePose(kt_bool useBarycenter) const
05250     {
05251       boost::shared_lock<boost::shared_mutex> lock(m_Lock);
05252       if (m_IsDirty)
05253       {
05254         // throw away constness and do an update!
05255         lock.unlock();
05256         boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
05257         const_cast<LocalizedRangeScan*>(this)->Update();
05258       }
05259 
05260       return useBarycenter ? GetBarycenterPose() : GetSensorPose();
05261     }
05262 
05267     inline Pose2 GetSensorPose() const
05268     {
05269       return GetSensorAt(m_CorrectedPose);
05270     }
05271 
05276     void SetSensorPose(const Pose2& rScanPose)
05277     {
05278       Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose();
05279       kt_double offsetLength = deviceOffsetPose2.GetPosition().Length();
05280       kt_double offsetHeading = deviceOffsetPose2.GetHeading();
05281       kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX());
05282       kt_double correctedHeading = math::NormalizeAngle(rScanPose.GetHeading());
05283       Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading),
05284                                       offsetLength * sin(correctedHeading + angleoffset - offsetHeading),
05285                                       offsetHeading);
05286 
05287       m_CorrectedPose = rScanPose - worldSensorOffset;
05288 
05289       Update();
05290     }
05291 
05297     inline Pose2 GetSensorAt(const Pose2& rPose) const
05298     {
05299       return Transform(rPose).TransformPose(GetLaserRangeFinder()->GetOffsetPose());
05300     }
05301 
05306     inline const BoundingBox2& GetBoundingBox() const
05307     {
05308       boost::shared_lock<boost::shared_mutex> lock(m_Lock);
05309       if (m_IsDirty)
05310       {
05311         // throw away constness and do an update!
05312         lock.unlock();
05313         boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
05314         const_cast<LocalizedRangeScan*>(this)->Update();
05315       }
05316 
05317       return m_BoundingBox;
05318     }
05319 
05323     inline const PointVectorDouble& GetPointReadings(kt_bool wantFiltered = false) const
05324     {
05325       boost::shared_lock<boost::shared_mutex> lock(m_Lock);
05326       if (m_IsDirty)
05327       {
05328         // throw away constness and do an update!
05329         lock.unlock();
05330         boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
05331         const_cast<LocalizedRangeScan*>(this)->Update();
05332       }
05333 
05334       if (wantFiltered == true)
05335       {
05336         return m_PointReadings;
05337       }
05338       else
05339       {
05340         return m_UnfilteredPointReadings;
05341       }
05342     }
05343 
05344   private:
05349     virtual void Update()
05350     {
05351       LaserRangeFinder* pLaserRangeFinder = GetLaserRangeFinder();
05352 
05353       if (pLaserRangeFinder != NULL)
05354       {
05355         m_PointReadings.clear();
05356         m_UnfilteredPointReadings.clear();
05357 
05358         kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold();
05359         kt_double minimumAngle = pLaserRangeFinder->GetMinimumAngle();
05360         kt_double angularResolution = pLaserRangeFinder->GetAngularResolution();
05361         Pose2 scanPose = GetSensorPose();
05362 
05363         // compute point readings
05364         Vector2<kt_double> rangePointsSum;
05365         kt_int32u beamNum = 0;
05366         for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++)
05367         {
05368           kt_double rangeReading = GetRangeReadings()[i];
05369           if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold))
05370           {
05371             kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
05372 
05373             Vector2<kt_double> point;
05374             point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
05375             point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
05376 
05377             m_UnfilteredPointReadings.push_back(point);
05378             continue;
05379           }
05380 
05381           kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
05382 
05383           Vector2<kt_double> point;
05384           point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
05385           point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
05386 
05387           m_PointReadings.push_back(point);
05388           m_UnfilteredPointReadings.push_back(point);
05389 
05390           rangePointsSum += point;
05391         }
05392 
05393         // compute barycenter
05394         kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
05395         if (nPoints != 0.0)
05396         {
05397           Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
05398           m_BarycenterPose = Pose2(averagePosition, 0.0);
05399         }
05400         else
05401         {
05402           m_BarycenterPose = scanPose;
05403         }
05404 
05405         // calculate bounding box of scan
05406         m_BoundingBox = BoundingBox2();
05407         m_BoundingBox.Add(scanPose.GetPosition());
05408         forEach(PointVectorDouble, &m_PointReadings)
05409         {
05410           m_BoundingBox.Add(*iter);
05411         }
05412       }
05413 
05414       m_IsDirty = false;
05415     }
05416 
05417   private:
05418     LocalizedRangeScan(const LocalizedRangeScan&);
05419     const LocalizedRangeScan& operator=(const LocalizedRangeScan&);
05420 
05421   private:
05425     Pose2 m_OdometricPose;
05426 
05430     Pose2 m_CorrectedPose;
05431 
05432   protected:
05436     Pose2 m_BarycenterPose;
05437 
05441     PointVectorDouble m_PointReadings;
05442 
05446     PointVectorDouble m_UnfilteredPointReadings;
05447 
05451     BoundingBox2 m_BoundingBox;
05452 
05456     kt_bool m_IsDirty;
05457   };  // LocalizedRangeScan
05458 
05462   typedef std::vector<LocalizedRangeScan*> LocalizedRangeScanVector;
05463 
05467 
05471   class LocalizedRangeScanWithPoints : public LocalizedRangeScan
05472   {
05473   public:
05474     // @cond EXCLUDE
05475     KARTO_Object(LocalizedRangeScanWithPoints)
05476     // @endcond
05477 
05478   public:
05483     LocalizedRangeScanWithPoints(const Name& rSensorName, const RangeReadingsVector& rReadings,
05484         const PointVectorDouble& rPoints)
05485         : m_Points(rPoints),
05486           LocalizedRangeScan(rSensorName, rReadings)
05487     {
05488     }
05489 
05493     virtual ~LocalizedRangeScanWithPoints()
05494     {
05495     }
05496 
05497   private:
05501     void Update()
05502     {
05503       m_PointReadings.clear();
05504       m_UnfilteredPointReadings.clear();
05505 
05506       Pose2 scanPose = GetSensorPose();
05507       Pose2 robotPose = GetCorrectedPose();
05508 
05509       // update point readings
05510       Vector2<kt_double> rangePointsSum;
05511       for (kt_int32u i = 0; i < m_Points.size(); i++)
05512       {
05513         // check if this has a NaN
05514         if (!std::isfinite(m_Points[i].GetX()) || !std::isfinite(m_Points[i].GetY()))
05515         {
05516           Vector2<kt_double> point(m_Points[i].GetX(), m_Points[i].GetY());
05517           m_UnfilteredPointReadings.push_back(point);
05518 
05519           continue;
05520         }
05521 
05522         // transform into world coords
05523         Pose2 pointPose(m_Points[i].GetX(), m_Points[i].GetY(), 0);
05524         Pose2 result = Transform(robotPose).TransformPose(pointPose);
05525         Vector2<kt_double> point(result.GetX(), result.GetY());
05526 
05527         m_PointReadings.push_back(point);
05528         m_UnfilteredPointReadings.push_back(point);
05529 
05530         rangePointsSum += point;
05531       }
05532 
05533       // compute barycenter
05534       kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
05535       if (nPoints != 0.0)
05536       {
05537         Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
05538         m_BarycenterPose = Pose2(averagePosition, 0.0);
05539       }
05540       else
05541       {
05542         m_BarycenterPose = scanPose;
05543       }
05544 
05545       // calculate bounding box of scan
05546       m_BoundingBox = BoundingBox2();
05547       m_BoundingBox.Add(scanPose.GetPosition());
05548       forEach(PointVectorDouble, &m_PointReadings)
05549       {
05550         m_BoundingBox.Add(*iter);
05551       }
05552 
05553       m_IsDirty = false;
05554     }
05555 
05556   private:
05557     LocalizedRangeScanWithPoints(const LocalizedRangeScanWithPoints&);
05558     const LocalizedRangeScanWithPoints& operator=(const LocalizedRangeScanWithPoints&);
05559 
05560   private:
05561     const PointVectorDouble m_Points;
05562   };  // LocalizedRangeScanWithPoints
05563 
05567 
05568   class OccupancyGrid;
05569 
05570   class KARTO_EXPORT CellUpdater : public Functor
05571   {
05572   public:
05573     CellUpdater(OccupancyGrid* pGrid)
05574       : m_pOccupancyGrid(pGrid)
05575     {
05576     }
05577 
05582     virtual void operator() (kt_int32u index);
05583 
05584   private:
05585     OccupancyGrid* m_pOccupancyGrid;
05586   };  // CellUpdater
05587 
05591   class OccupancyGrid : public Grid<kt_int8u>
05592   {
05593     friend class CellUpdater;
05594     friend class IncrementalOccupancyGrid;
05595 
05596   public:
05604     OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2<kt_double>& rOffset, kt_double resolution)
05605       : Grid<kt_int8u>(width, height)
05606       , m_pCellPassCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
05607       , m_pCellHitsCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
05608       , m_pCellUpdater(NULL)
05609     {
05610       m_pCellUpdater = new CellUpdater(this);
05611 
05612       if (karto::math::DoubleEqual(resolution, 0.0))
05613       {
05614         throw Exception("Resolution cannot be 0");
05615       }
05616 
05617       m_pMinPassThrough = new Parameter<kt_int32u>("MinPassThrough", 2);
05618       m_pOccupancyThreshold = new Parameter<kt_double>("OccupancyThreshold", 0.1);
05619 
05620       GetCoordinateConverter()->SetScale(1.0 / resolution);
05621       GetCoordinateConverter()->SetOffset(rOffset);
05622     }
05623 
05627     virtual ~OccupancyGrid()
05628     {
05629       delete m_pCellUpdater;
05630 
05631       delete m_pCellPassCnt;
05632       delete m_pCellHitsCnt;
05633 
05634       delete m_pMinPassThrough;
05635       delete m_pOccupancyThreshold;
05636     }
05637 
05638   public:
05644     static OccupancyGrid* CreateFromScans(const LocalizedRangeScanVector& rScans, kt_double resolution)
05645     {
05646       if (rScans.empty())
05647       {
05648         return NULL;
05649       }
05650 
05651       kt_int32s width, height;
05652       Vector2<kt_double> offset;
05653       ComputeDimensions(rScans, resolution, width, height, offset);
05654       OccupancyGrid* pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);
05655       pOccupancyGrid->CreateFromScans(rScans);
05656 
05657       return pOccupancyGrid;
05658     }
05659 
05664     OccupancyGrid* Clone() const
05665     {
05666       OccupancyGrid* pOccupancyGrid = new OccupancyGrid(GetWidth(),
05667                                                         GetHeight(),
05668                                                         GetCoordinateConverter()->GetOffset(),
05669                                                         1.0 / GetCoordinateConverter()->GetScale());
05670       memcpy(pOccupancyGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
05671 
05672       pOccupancyGrid->GetCoordinateConverter()->SetSize(GetCoordinateConverter()->GetSize());
05673       pOccupancyGrid->m_pCellPassCnt = m_pCellPassCnt->Clone();
05674       pOccupancyGrid->m_pCellHitsCnt = m_pCellHitsCnt->Clone();
05675 
05676       return pOccupancyGrid;
05677     }
05678 
05684     virtual kt_bool IsFree(const Vector2<kt_int32s>& rPose) const
05685     {
05686       kt_int8u* pOffsets = reinterpret_cast<kt_int8u*>(GetDataPointer(rPose));
05687       if (*pOffsets == GridStates_Free)
05688       {
05689         return true;
05690       }
05691 
05692       return false;
05693     }
05694 
05702     virtual kt_double RayCast(const Pose2& rPose2, kt_double maxRange) const
05703     {
05704       double scale = GetCoordinateConverter()->GetScale();
05705 
05706       kt_double x = rPose2.GetX();
05707       kt_double y = rPose2.GetY();
05708       kt_double theta = rPose2.GetHeading();
05709 
05710       kt_double sinTheta = sin(theta);
05711       kt_double cosTheta = cos(theta);
05712 
05713       kt_double xStop = x + maxRange * cosTheta;
05714       kt_double xSteps = 1 + fabs(xStop - x) * scale;
05715 
05716       kt_double yStop = y + maxRange * sinTheta;
05717       kt_double ySteps = 1 + fabs(yStop - y) * scale;
05718 
05719       kt_double steps = math::Maximum(xSteps, ySteps);
05720       kt_double delta = maxRange / steps;
05721       kt_double distance = delta;
05722 
05723       for (kt_int32u i = 1; i < steps; i++)
05724       {
05725         kt_double x1 = x + distance * cosTheta;
05726         kt_double y1 = y + distance * sinTheta;
05727 
05728         Vector2<kt_int32s> gridIndex = WorldToGrid(Vector2<kt_double>(x1, y1));
05729         if (IsValidGridIndex(gridIndex) && IsFree(gridIndex))
05730         {
05731           distance = (i + 1) * delta;
05732         }
05733         else
05734         {
05735           break;
05736         }
05737       }
05738 
05739       return (distance < maxRange)? distance : maxRange;
05740     }
05741 
05747     void SetMinPassThrough(kt_int32u count)
05748     {
05749       m_pMinPassThrough->SetValue(count);
05750     }
05751 
05756     void SetOccupancyThreshold(kt_double thresh)
05757     {
05758       m_pOccupancyThreshold->SetValue(thresh);
05759     }
05760 
05761   protected:
05766     virtual Grid<kt_int32u>* GetCellHitsCounts()
05767     {
05768       return m_pCellHitsCnt;
05769     }
05770 
05775     virtual Grid<kt_int32u>* GetCellPassCounts()
05776     {
05777       return m_pCellPassCnt;
05778     }
05779 
05780   protected:
05789     static void ComputeDimensions(const LocalizedRangeScanVector& rScans,
05790                                   kt_double resolution,
05791                                   kt_int32s& rWidth,
05792                                   kt_int32s& rHeight,
05793                                   Vector2<kt_double>& rOffset)
05794     {
05795       BoundingBox2 boundingBox;
05796       const_forEach(LocalizedRangeScanVector, &rScans)
05797       {
05798         boundingBox.Add((*iter)->GetBoundingBox());
05799       }
05800 
05801       kt_double scale = 1.0 / resolution;
05802       Size2<kt_double> size = boundingBox.GetSize();
05803 
05804       rWidth = static_cast<kt_int32s>(math::Round(size.GetWidth() * scale));
05805       rHeight = static_cast<kt_int32s>(math::Round(size.GetHeight() * scale));
05806       rOffset = boundingBox.GetMinimum();
05807     }
05808 
05813     virtual void CreateFromScans(const LocalizedRangeScanVector& rScans)
05814     {
05815       m_pCellPassCnt->Resize(GetWidth(), GetHeight());
05816       m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
05817 
05818       m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
05819       m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
05820 
05821       const_forEach(LocalizedRangeScanVector, &rScans)
05822       {
05823         LocalizedRangeScan* pScan = *iter;
05824         AddScan(pScan);
05825       }
05826 
05827       Update();
05828     }
05829 
05837     virtual kt_bool AddScan(LocalizedRangeScan* pScan, kt_bool doUpdate = false)
05838     {
05839       kt_double rangeThreshold = pScan->GetLaserRangeFinder()->GetRangeThreshold();
05840       kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
05841       kt_double minRange = pScan->GetLaserRangeFinder()->GetMinimumRange();
05842 
05843       Vector2<kt_double> scanPosition = pScan->GetSensorPose().GetPosition();
05844 
05845       // get scan point readings
05846       const PointVectorDouble& rPointReadings = pScan->GetPointReadings(false);
05847 
05848       kt_bool isAllInMap = true;
05849 
05850       // draw lines from scan position to all point readings
05851       int pointIndex = 0;
05852       const_forEachAs(PointVectorDouble, &rPointReadings, pointsIter)
05853       {
05854         Vector2<kt_double> point = *pointsIter;
05855         kt_double rangeReading = pScan->GetRangeReadings()[pointIndex];
05856         kt_bool isEndPointValid = rangeReading < (rangeThreshold - KT_TOLERANCE);
05857 
05858         if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading))
05859         {
05860           // ignore these readings
05861           pointIndex++;
05862           continue;
05863         }
05864         else if (rangeReading >= rangeThreshold)
05865         {
05866           // trace up to range reading
05867           kt_double ratio = rangeThreshold / rangeReading;
05868           kt_double dx = point.GetX() - scanPosition.GetX();
05869           kt_double dy = point.GetY() - scanPosition.GetY();
05870           point.SetX(scanPosition.GetX() + ratio * dx);
05871           point.SetY(scanPosition.GetY() + ratio * dy);
05872         }
05873 
05874         kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate);
05875         if (!isInMap)
05876         {
05877           isAllInMap = false;
05878         }
05879 
05880         pointIndex++;
05881       }
05882 
05883       return isAllInMap;
05884     }
05885 
05895     virtual kt_bool RayTrace(const Vector2<kt_double>& rWorldFrom,
05896                              const Vector2<kt_double>& rWorldTo,
05897                              kt_bool isEndPointValid,
05898                              kt_bool doUpdate = false)
05899     {
05900       assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
05901 
05902       Vector2<kt_int32s> gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom);
05903       Vector2<kt_int32s> gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo);
05904 
05905       CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL;
05906       m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater);
05907 
05908       // for the end point
05909       if (isEndPointValid)
05910       {
05911         if (m_pCellPassCnt->IsValidGridIndex(gridTo))
05912         {
05913           kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false);
05914 
05915           kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
05916           kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
05917 
05918           // increment cell pass through and hit count
05919           pCellPassCntPtr[index]++;
05920           pCellHitCntPtr[index]++;
05921 
05922           if (doUpdate)
05923           {
05924             (*m_pCellUpdater)(index);
05925           }
05926         }
05927       }
05928 
05929       return m_pCellPassCnt->IsValidGridIndex(gridTo);
05930     }
05931 
05938     virtual void UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
05939     {
05940       if (cellPassCnt > m_pMinPassThrough->GetValue())
05941       {
05942         kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);
05943 
05944         if (hitRatio > m_pOccupancyThreshold->GetValue())
05945         {
05946           *pCell = GridStates_Occupied;
05947         }
05948         else
05949         {
05950           *pCell = GridStates_Free;
05951         }
05952       }
05953     }
05954 
05958     virtual void Update()
05959     {
05960       assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
05961 
05962       // clear grid
05963       Clear();
05964 
05965       // set occupancy status of cells
05966       kt_int8u* pDataPtr = GetDataPointer();
05967       kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
05968       kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
05969 
05970       kt_int32u nBytes = GetDataSize();
05971       for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++)
05972       {
05973         UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);
05974       }
05975     }
05976 
05982     virtual void Resize(kt_int32s width, kt_int32s height)
05983     {
05984       Grid<kt_int8u>::Resize(width, height);
05985       m_pCellPassCnt->Resize(width, height);
05986       m_pCellHitsCnt->Resize(width, height);
05987     }
05988 
05989   protected:
05993     Grid<kt_int32u>* m_pCellPassCnt;
05994 
05998     Grid<kt_int32u>* m_pCellHitsCnt;
05999 
06000   private:
06004     OccupancyGrid(const OccupancyGrid&);
06005 
06009     const OccupancyGrid& operator=(const OccupancyGrid&);
06010 
06011   private:
06012     CellUpdater* m_pCellUpdater;
06013 
06015     // NOTE: These two values are dependent on the resolution.  If the resolution is too small,
06016     // then not many beams will hit the cell!
06017 
06018     // Number of beams that must pass through a cell before it will be considered to be occupied
06019     // or unoccupied.  This prevents stray beams from messing up the map.
06020     Parameter<kt_int32u>* m_pMinPassThrough;
06021 
06022     // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied
06023     Parameter<kt_double>* m_pOccupancyThreshold;
06024   };  // OccupancyGrid
06025 
06029 
06034   class DatasetInfo : public Object
06035   {
06036   public:
06037     // @cond EXCLUDE
06038     KARTO_Object(DatasetInfo)
06039     // @endcond
06040 
06041   public:
06042     DatasetInfo()
06043       : Object()
06044     {
06045       m_pTitle = new Parameter<std::string>("Title", "", GetParameterManager());
06046       m_pAuthor = new Parameter<std::string>("Author", "", GetParameterManager());
06047       m_pDescription = new Parameter<std::string>("Description", "", GetParameterManager());
06048       m_pCopyright = new Parameter<std::string>("Copyright", "", GetParameterManager());
06049     }
06050 
06051     virtual ~DatasetInfo()
06052     {
06053     }
06054 
06055   public:
06059     const std::string& GetTitle() const
06060     {
06061       return m_pTitle->GetValue();
06062     }
06063 
06067     const std::string& GetAuthor() const
06068     {
06069       return m_pAuthor->GetValue();
06070     }
06071 
06075     const std::string& GetDescription() const
06076     {
06077       return m_pDescription->GetValue();
06078     }
06079 
06083     const std::string& GetCopyright() const
06084     {
06085       return m_pCopyright->GetValue();
06086     }
06087 
06088   private:
06089     DatasetInfo(const DatasetInfo&);
06090     const DatasetInfo& operator=(const DatasetInfo&);
06091 
06092   private:
06093     Parameter<std::string>* m_pTitle;
06094     Parameter<std::string>* m_pAuthor;
06095     Parameter<std::string>* m_pDescription;
06096     Parameter<std::string>* m_pCopyright;
06097   };  // class DatasetInfo
06098 
06102 
06107   class Dataset
06108   {
06109   public:
06113     Dataset()
06114       : m_pDatasetInfo(NULL)
06115     {
06116     }
06117 
06121     virtual ~Dataset()
06122     {
06123       Clear();
06124     }
06125 
06126   public:
06131     void Add(Object* pObject)
06132     {
06133       if (pObject != NULL)
06134       {
06135         if (dynamic_cast<Sensor*>(pObject))
06136         {
06137           Sensor* pSensor = dynamic_cast<Sensor*>(pObject);
06138           if (pSensor != NULL)
06139           {
06140             m_SensorNameLookup[pSensor->GetName()] = pSensor;
06141 
06142             karto::SensorManager::GetInstance()->RegisterSensor(pSensor);
06143           }
06144 
06145           m_Objects.push_back(pObject);
06146         }
06147         else if (dynamic_cast<SensorData*>(pObject))
06148         {
06149           SensorData* pSensorData = dynamic_cast<SensorData*>(pObject);
06150           m_Objects.push_back(pSensorData);
06151         }
06152         else if (dynamic_cast<DatasetInfo*>(pObject))
06153         {
06154           m_pDatasetInfo = dynamic_cast<DatasetInfo*>(pObject);
06155         }
06156         else
06157         {
06158           m_Objects.push_back(pObject);
06159         }
06160       }
06161     }
06162 
06167     inline const ObjectVector& GetObjects() const
06168     {
06169       return m_Objects;
06170     }
06171 
06176     inline DatasetInfo* GetDatasetInfo()
06177     {
06178       return m_pDatasetInfo;
06179     }
06180 
06184     virtual void Clear()
06185     {
06186       for (std::map<Name, Sensor*>::iterator iter = m_SensorNameLookup.begin(); iter != m_SensorNameLookup.end(); ++iter)
06187       {
06188         karto::SensorManager::GetInstance()->UnregisterSensor(iter->second);
06189       }
06190 
06191       forEach(ObjectVector, &m_Objects)
06192       {
06193         delete *iter;
06194       }
06195 
06196       m_Objects.clear();
06197 
06198       if (m_pDatasetInfo != NULL)
06199       {
06200         delete m_pDatasetInfo;
06201         m_pDatasetInfo = NULL;
06202       }
06203     }
06204 
06205   private:
06206     std::map<Name, Sensor*> m_SensorNameLookup;
06207     ObjectVector m_Objects;
06208     DatasetInfo* m_pDatasetInfo;
06209   };  // Dataset
06210 
06214 
06219   class LookupArray
06220   {
06221   public:
06225     LookupArray()
06226       : m_pArray(NULL)
06227       , m_Capacity(0)
06228       , m_Size(0)
06229     {
06230     }
06231 
06235     virtual ~LookupArray()
06236     {
06237       assert(m_pArray != NULL);
06238 
06239       delete[] m_pArray;
06240       m_pArray = NULL;
06241     }
06242 
06243   public:
06247     void Clear()
06248     {
06249       memset(m_pArray, 0, sizeof(kt_int32s) * m_Capacity);
06250     }
06251 
06256     kt_int32u GetSize() const
06257     {
06258       return m_Size;
06259     }
06260 
06265     void SetSize(kt_int32u size)
06266     {
06267       assert(size != 0);
06268 
06269       if (size > m_Capacity)
06270       {
06271         if (m_pArray != NULL)
06272         {
06273           delete [] m_pArray;
06274         }
06275         m_Capacity = size;
06276         m_pArray = new kt_int32s[m_Capacity];
06277       }
06278 
06279       m_Size = size;
06280     }
06281 
06287     inline kt_int32s& operator [] (kt_int32u index)
06288     {
06289       assert(index < m_Size);
06290 
06291       return m_pArray[index];
06292     }
06293 
06299     inline kt_int32s operator [] (kt_int32u index) const
06300     {
06301       assert(index < m_Size);
06302 
06303       return m_pArray[index];
06304     }
06305 
06310     inline kt_int32s* GetArrayPointer()
06311     {
06312       return m_pArray;
06313     }
06314 
06319     inline kt_int32s* GetArrayPointer() const
06320     {
06321       return m_pArray;
06322     }
06323 
06324   private:
06325     kt_int32s* m_pArray;
06326     kt_int32u m_Capacity;
06327     kt_int32u m_Size;
06328   };  // LookupArray
06329 
06333 
06347   template<typename T>
06348   class GridIndexLookup
06349   {
06350   public:
06355     GridIndexLookup(Grid<T>* pGrid)
06356       : m_pGrid(pGrid)
06357       , m_Capacity(0)
06358       , m_Size(0)
06359       , m_ppLookupArray(NULL)
06360     {
06361     }
06362 
06366     virtual ~GridIndexLookup()
06367     {
06368       DestroyArrays();
06369     }
06370 
06371   public:
06377     const LookupArray* GetLookupArray(kt_int32u index) const
06378     {
06379       assert(math::IsUpTo(index, m_Size));
06380 
06381       return m_ppLookupArray[index];
06382     }
06383 
06388     const std::vector<kt_double>& GetAngles() const
06389     {
06390       return m_Angles;
06391     }
06392 
06400     void ComputeOffsets(LocalizedRangeScan* pScan,
06401                         kt_double angleCenter,
06402                         kt_double angleOffset,
06403                         kt_double angleResolution)
06404     {
06405       assert(angleOffset != 0.0);
06406       assert(angleResolution != 0.0);
06407 
06408       kt_int32u nAngles = static_cast<kt_int32u>(math::Round(angleOffset * 2.0 / angleResolution) + 1);
06409       SetSize(nAngles);
06410 
06412       // convert points into local coordinates of scan pose
06413 
06414       const PointVectorDouble& rPointReadings = pScan->GetPointReadings();
06415 
06416       // compute transform to scan pose
06417       Transform transform(pScan->GetSensorPose());
06418 
06419       Pose2Vector localPoints;
06420       const_forEach(PointVectorDouble, &rPointReadings)
06421       {
06422         // do inverse transform to get points in local coordinates
06423         Pose2 vec = transform.InverseTransformPose(Pose2(*iter, 0.0));
06424         localPoints.push_back(vec);
06425       }
06426 
06428       // create lookup array for different angles
06429       kt_double angle = 0.0;
06430       kt_double startAngle = angleCenter - angleOffset;
06431       for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
06432       {
06433         angle = startAngle + angleIndex * angleResolution;
06434         ComputeOffsets(angleIndex, angle, localPoints, pScan);
06435       }
06436       // assert(math::DoubleEqual(angle, angleCenter + angleOffset));
06437     }
06438 
06439   private:
06446     void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2Vector& rLocalPoints, LocalizedRangeScan* pScan)
06447     {
06448       m_ppLookupArray[angleIndex]->SetSize(static_cast<kt_int32u>(rLocalPoints.size()));
06449       m_Angles.at(angleIndex) = angle;
06450 
06451       // set up point array by computing relative offsets to points readings
06452       // when rotated by given angle
06453 
06454       const Vector2<kt_double>& rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset();
06455 
06456       kt_double cosine = cos(angle);
06457       kt_double sine = sin(angle);
06458 
06459       kt_int32u readingIndex = 0;
06460 
06461       kt_int32s* pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer();
06462 
06463       kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
06464 
06465       const_forEach(Pose2Vector, &rLocalPoints)
06466       {
06467         const Vector2<kt_double>& rPosition = iter->GetPosition();
06468 
06469         if (std::isnan(pScan->GetRangeReadings()[readingIndex]) || std::isinf(pScan->GetRangeReadings()[readingIndex]))
06470         {
06471           pAngleIndexPointer[readingIndex] = INVALID_SCAN;
06472           readingIndex++;
06473           continue;
06474         }
06475 
06476 
06477         // counterclockwise rotation and that rotation is about the origin (0, 0).
06478         Vector2<kt_double> offset;
06479         offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY());
06480         offset.SetY(sine * rPosition.GetX() + cosine * rPosition.GetY());
06481 
06482         // have to compensate for the grid offset when getting the grid index
06483         Vector2<kt_int32s> gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset);
06484 
06485         // use base GridIndex to ignore ROI
06486         kt_int32s lookupIndex = m_pGrid->Grid<T>::GridIndex(gridPoint, false);
06487 
06488         pAngleIndexPointer[readingIndex] = lookupIndex;
06489 
06490         readingIndex++;
06491       }
06492       assert(readingIndex == rLocalPoints.size());
06493     }
06494 
06499     void SetSize(kt_int32u size)
06500     {
06501       assert(size != 0);
06502 
06503       if (size > m_Capacity)
06504       {
06505         if (m_ppLookupArray != NULL)
06506         {
06507           DestroyArrays();
06508         }
06509 
06510         m_Capacity = size;
06511         m_ppLookupArray = new LookupArray*[m_Capacity];
06512         for (kt_int32u i = 0; i < m_Capacity; i++)
06513         {
06514           m_ppLookupArray[i] = new LookupArray();
06515         }
06516       }
06517 
06518       m_Size = size;
06519 
06520       m_Angles.resize(size);
06521     }
06522 
06526     void DestroyArrays()
06527     {
06528       for (kt_int32u i = 0; i < m_Capacity; i++)
06529       {
06530         delete m_ppLookupArray[i];
06531       }
06532 
06533       delete[] m_ppLookupArray;
06534       m_ppLookupArray = NULL;
06535     }
06536 
06537   private:
06538     Grid<T>* m_pGrid;
06539 
06540     kt_int32u m_Capacity;
06541     kt_int32u m_Size;
06542 
06543     LookupArray **m_ppLookupArray;
06544 
06545     // for sanity check
06546     std::vector<kt_double> m_Angles;
06547   };  // class GridIndexLookup
06548 
06552 
06553   inline Pose2::Pose2(const Pose3& rPose)
06554     : m_Position(rPose.GetPosition().GetX(), rPose.GetPosition().GetY())
06555   {
06556     kt_double t1, t2;
06557 
06558     // calculates heading from orientation
06559     rPose.GetOrientation().ToEulerAngles(m_Heading, t1, t2);
06560   }
06561 
06565 
06566   // @cond EXCLUDE
06567 
06568   template<typename T>
06569   inline void Object::SetParameter(const std::string& rName, T value)
06570   {
06571     AbstractParameter* pParameter = GetParameter(rName);
06572     if (pParameter != NULL)
06573     {
06574       std::stringstream stream;
06575       stream << value;
06576       pParameter->SetValueFromString(stream.str());
06577     }
06578     else
06579     {
06580       throw Exception("Parameter does not exist:  " + rName);
06581     }
06582   }
06583 
06584   template<>
06585   inline void Object::SetParameter(const std::string& rName, kt_bool value)
06586   {
06587     AbstractParameter* pParameter = GetParameter(rName);
06588     if (pParameter != NULL)
06589     {
06590       pParameter->SetValueFromString(value ? "true" : "false");
06591     }
06592     else
06593     {
06594       throw Exception("Parameter does not exist:  " + rName);
06595     }
06596   }
06597 
06598   // @endcond
06599 
06601 }  // namespace karto
06602 
06603 #endif  // OPEN_KARTO_KARTO_H


open_karto
Author(s):
autogenerated on Thu Jun 6 2019 21:02:56