Karto.h
Go to the documentation of this file.
1 /*
2  * Copyright 2010 SRI International
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #ifndef karto_sdk_KARTO_H
19 #define karto_sdk_KARTO_H
20 
21 #include <string>
22 #include <fstream>
23 #include <limits>
24 #include <algorithm>
25 #include <map>
26 #include <vector>
27 #include <iostream>
28 #include <iomanip>
29 #include <sstream>
30 #include <stdexcept>
31 
32 #include <math.h>
33 #include <float.h>
34 #include <stdio.h>
35 #include <assert.h>
36 #include <string.h>
37 #include <boost/thread.hpp>
38 
39 #include <boost/serialization/base_object.hpp>
40 #include <boost/serialization/nvp.hpp>
41 #include <boost/serialization/utility.hpp>
42 #include <boost/serialization/export.hpp>
43 #include <boost/type_traits/is_abstract.hpp>
44 #include <boost/archive/binary_iarchive.hpp>
45 #include <boost/archive/binary_oarchive.hpp>
46 #include <boost/serialization/map.hpp>
47 #include <boost/serialization/vector.hpp>
48 #include <boost/serialization/string.hpp>
49 #include <boost/serialization/array.hpp>
50 #include <boost/version.hpp>
51 
52 #ifdef USE_POCO
53 #include <Poco/Mutex.h>
54 #endif
55 
56 #include <karto_sdk/Math.h>
57 #include <karto_sdk/Macros.h>
58 
59 #define KARTO_Object(name) \
60  virtual const char* GetClassName() const { return #name; } \
61  virtual kt_objecttype GetObjectType() const { return ObjectType_##name; }
62 
64 
65 const kt_objecttype ObjectType_None = 0x00000000;
66 const kt_objecttype ObjectType_Sensor = 0x00001000;
69 const kt_objecttype ObjectType_Misc = 0x10000000;
70 
74 
80 
85 
86 namespace karto
87 {
88 
93 
98  {
99  public:
105  Exception(const std::string& rMessage = "Karto Exception", kt_int32s errorCode = 0)
106  : m_Message(rMessage)
107  , m_ErrorCode(errorCode)
108  {
109  }
110 
114  Exception(const Exception& rException)
115  : m_Message(rException.m_Message)
116  , m_ErrorCode(rException.m_ErrorCode)
117  {
118  }
119 
123  virtual ~Exception()
124  {
125  }
126 
127  public:
131  Exception& operator = (const Exception& rException)
132  {
133  m_Message = rException.m_Message;
134  m_ErrorCode = rException.m_ErrorCode;
135 
136  return *this;
137  }
138 
139  public:
144  const std::string& GetErrorMessage() const
145  {
146  return m_Message;
147  }
148 
154  {
155  return m_ErrorCode;
156  }
157 
158  public:
164  friend KARTO_EXPORT std::ostream& operator << (std::ostream& rStream, Exception& rException);
165 
166  private:
167  std::string m_Message;
169  }; // class Exception
170 
174 
180  {
181  private:
182  NonCopyable(const NonCopyable&) = delete;
183  const NonCopyable& operator=(const NonCopyable&) = delete;
184 
185  public:
187  {
188  }
189 
190  virtual ~NonCopyable()
191  {
192  }
193 
194  friend class boost::serialization::access;
195  template<class Archive>
196  void serialize(Archive &ar, const unsigned int version)
197  {
198  }
199  }; // class NonCopyable
200 
204 
208  template <class T>
209  class Singleton
210  {
211  public:
216  : m_pPointer(NULL)
217  {
218  }
219 
223  virtual ~Singleton()
224  {
225  delete m_pPointer;
226  }
227 
232  T* Get()
233  {
234 #ifdef USE_POCO
235  Poco::FastMutex::ScopedLock lock(m_Mutex);
236 #endif
237  if (m_pPointer == NULL)
238  {
239  m_pPointer = new T;
240  }
241 
242  return m_pPointer;
243  }
244 
245  private:
247 
248 #ifdef USE_POCO
249  Poco::FastMutex m_Mutex;
250 #endif
251 
252  private:
253  Singleton(const Singleton&);
254  const Singleton& operator=(const Singleton&);
255  };
256 
260 
265  {
266  public:
270  virtual void operator() (kt_int32u) {};
271  }; // Functor
272 
276 
278 
282  typedef std::vector<AbstractParameter*> ParameterVector;
283 
288  {
289  public:
294  {
295  }
296 
301  {
302  Clear();
303  }
304 
305  public:
310  void Add(AbstractParameter* pParameter);
311 
317  AbstractParameter* Get(const std::string& rName)
318  {
319  if (m_ParameterLookup.find(rName) != m_ParameterLookup.end())
320  {
321  return m_ParameterLookup[rName];
322  }
323 
324  std::cout << "Unknown parameter: " << rName << std::endl;
325 
326  return NULL;
327  }
328 
332  void Clear();
333 
338  inline const ParameterVector& GetParameterVector() const
339  {
340  return m_Parameters;
341  }
342 
343  public:
349  AbstractParameter* operator() (const std::string& rName)
350  {
351  return Get(rName);
352  }
353 
358  private:
360  const ParameterManager& operator=(const ParameterManager&);
361 
362  private:
363  ParameterVector m_Parameters;
364  std::map<std::string, AbstractParameter*> m_ParameterLookup;
365 
366  friend class boost::serialization::access;
367  template<class Archive>
368  void serialize(Archive &ar, const unsigned int version)
369  {
370  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NonCopyable);
371  ar & BOOST_SERIALIZATION_NVP(m_Parameters);
372  ar & BOOST_SERIALIZATION_NVP(m_ParameterLookup);
373  }
374 
375  }; // ParameterManager
376 
380 
381  // valid names
382  // 'Test' -- no scope
383  // '/Test' -- no scope will be parsed to 'Test'
384  // '/scope/Test' - 'scope' scope and 'Test' name
385  // '/scope/name/Test' - 'scope/name' scope and 'Test' name
386  //
387  class Name
388  {
389  public:
394  {
395  }
396 
400  Name(const std::string& rName)
401  {
402  Parse(rName);
403  }
404 
408  Name(const Name& rOther)
409  : m_Scope(rOther.m_Scope), m_Name(rOther.m_Name)
410  {
411  }
412 
416  virtual ~Name()
417  {
418  }
419 
420  public:
425  inline const std::string& GetName() const
426  {
427  return m_Name;
428  }
429 
434  inline void SetName(const std::string& rName)
435  {
436  std::string::size_type pos = rName.find_last_of('/');
437  if (pos != 0 && pos != std::string::npos)
438  {
439  throw Exception("Name can't contain a scope!");
440  }
441 
442  m_Name = rName;
443  }
444 
449  inline const std::string& GetScope() const
450  {
451  return m_Scope;
452  }
453 
458  inline void SetScope(const std::string& rScope)
459  {
460  m_Scope = rScope;
461  }
462 
467  inline std::string ToString() const
468  {
469  if (m_Scope.empty())
470  {
471  return m_Name;
472  }
473  else
474  {
475  std::string name;
476  name.append("/");
477  name.append(m_Scope);
478  name.append("/");
479  name.append(m_Name);
480 
481  return name;
482  }
483  }
484 
485  public:
489  Name& operator = (const Name& rOther)
490  {
491  if (&rOther != this)
492  {
493  m_Name = rOther.m_Name;
494  m_Scope = rOther.m_Scope;
495  }
496 
497  return *this;
498  }
499 
503  kt_bool operator == (const Name& rOther) const
504  {
505  return (m_Name == rOther.m_Name) && (m_Scope == rOther.m_Scope);
506  }
507 
511  kt_bool operator != (const Name& rOther) const
512  {
513  return !(*this == rOther);
514  }
515 
519  kt_bool operator < (const Name& rOther) const
520  {
521  return this->ToString() < rOther.ToString();
522  }
523 
529  friend inline std::ostream& operator << (std::ostream& rStream, const Name& rName)
530  {
531  rStream << rName.ToString();
532  return rStream;
533  }
534 
535  private:
540  void Parse(const std::string& rName)
541  {
542  std::string::size_type pos = rName.find_last_of('/');
543 
544  if (pos == std::string::npos)
545  {
546  m_Name = rName;
547  }
548  else
549  {
550  m_Scope = rName.substr(0, pos);
551  m_Name = rName.substr(pos+1, rName.size());
552 
553  // remove '/' from m_Scope if first!!
554  if (m_Scope.size() > 0 && m_Scope[0] == '/')
555  {
556  m_Scope = m_Scope.substr(1, m_Scope.size());
557  }
558  }
559  }
560 
565  void Validate(const std::string& rName)
566  {
567  if (rName.empty())
568  {
569  return;
570  }
571 
572  char c = rName[0];
573  if (IsValidFirst(c))
574  {
575  for (size_t i = 1; i < rName.size(); ++i)
576  {
577  c = rName[i];
578  if (!IsValid(c))
579  {
580  throw Exception("Invalid character in name. Valid characters must be within the ranges A-Z, a-z, 0-9, '/', '_' and '-'.");
581  }
582  }
583  }
584  else
585  {
586  throw Exception("Invalid first character in name. Valid characters must be within the ranges A-Z, a-z, and '/'.");
587  }
588  }
589 
595  inline kt_bool IsValidFirst(char c)
596  {
597  return (isalpha(c) || c == '/');
598  }
599 
605  inline kt_bool IsValid(char c)
606  {
607  return (isalnum(c) || c == '/' || c == '_' || c == '-');
608  }
609 
610  private:
611  std::string m_Name;
612  std::string m_Scope;
616  friend class boost::serialization::access;
617  template<class Archive>
618  void serialize(Archive &ar, const unsigned int version)
619  {
620  ar & BOOST_SERIALIZATION_NVP(m_Name);
621  ar & BOOST_SERIALIZATION_NVP(m_Scope);
622  }
623  };
624 
628 
633  {
634  public:
638  Object();
639 
644  Object(const Name& rName);
645 
649  virtual ~Object();
650 
651  public:
656  inline const Name& GetName() const
657  {
658  return m_Name;
659  }
660 
665  virtual const char* GetClassName() const = 0;
666 
671  virtual kt_objecttype GetObjectType() const = 0;
672 
678  {
679  return m_pParameterManager;
680  }
681 
687  inline AbstractParameter* GetParameter(const std::string& rName) const
688  {
689  return m_pParameterManager->Get(rName);
690  }
691 
697  template<typename T>
698  inline void SetParameter(const std::string& rName, T value);
699 
704  inline const ParameterVector& GetParameters() const
705  {
706  return m_pParameterManager->GetParameterVector();
707  }
708 
709  Object(const Object&);
710  const Object& operator=(const Object&);
711 
712  private:
718  friend class boost::serialization::access;
719  template<class Archive>
720  void serialize(Archive &ar, const unsigned int version)
721  {
722  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NonCopyable);
723  ar & BOOST_SERIALIZATION_NVP(m_pParameterManager);
724  ar & BOOST_SERIALIZATION_NVP(m_Name);
725  }
726  };
727  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Object)
728 
729 
732  typedef std::vector<Object*> ObjectVector;
733  typedef std::map<kt_int32s, Object*> DataMap;
734 
738 
744  inline kt_bool IsSensor(Object* pObject)
745  {
746  return (pObject->GetObjectType() & ObjectType_Sensor) == ObjectType_Sensor;
747  }
748 
754  inline kt_bool IsSensorData(Object* pObject)
755  {
757  }
758 
765  {
767  }
768 
775  {
777  }
778 
785  {
787  }
788 
794  inline kt_bool IsParameters(Object* pObject)
795  {
797  }
798 
804  inline kt_bool IsDatasetInfo(Object* pObject)
805  {
807  }
808 
812 
816  class KARTO_EXPORT Module : public Object
817  {
818  public:
819  // @cond EXCLUDE
821  // @endcond
822 
823  public:
828  Module(const std::string& rName);
829 
833  virtual ~Module();
834 
835  public:
839  virtual void Reset() = 0;
840 
845  {
846  return false;
847  }
848 
849  private:
850  Module(const Module&);
851  const Module& operator=(const Module&);
852 
853  friend class boost::serialization::access;
854  template<class Archive>
855  void serialize(Archive &ar, const unsigned int version)
856  {
857  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
858  }
859  };
860  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Module)
861 
862 
863 
869  template<typename T>
870  class Size2
871  {
872  public:
877  : m_Width(0)
878  , m_Height(0)
879  {
880  }
881 
887  Size2(T width, T height)
888  : m_Width(width)
889  , m_Height(height)
890  {
891  }
892 
897  Size2(const Size2& rOther)
898  : m_Width(rOther.m_Width)
899  , m_Height(rOther.m_Height)
900  {
901  }
902 
903  public:
908  inline const T GetWidth() const
909  {
910  return m_Width;
911  }
912 
917  inline void SetWidth(T width)
918  {
919  m_Width = width;
920  }
921 
926  inline const T GetHeight() const
927  {
928  return m_Height;
929  }
930 
935  inline void SetHeight(T height)
936  {
937  m_Height = height;
938  }
939 
943  inline Size2& operator = (const Size2& rOther)
944  {
945  m_Width = rOther.m_Width;
946  m_Height = rOther.m_Height;
947 
948  return(*this);
949  }
950 
954  inline kt_bool operator == (const Size2& rOther) const
955  {
956  return (m_Width == rOther.m_Width && m_Height == rOther.m_Height);
957  }
958 
962  inline kt_bool operator != (const Size2& rOther) const
963  {
964  return (m_Width != rOther.m_Width || m_Height != rOther.m_Height);
965  }
966 
972  friend inline std::ostream& operator << (std::ostream& rStream, const Size2& rSize)
973  {
974  rStream << "(" << rSize.m_Width << ", " << rSize.m_Height << ")";
975  return rStream;
976  }
977 
978  private:
981  friend class boost::serialization::access;
982  template<class Archive>
983  void serialize(Archive &ar, const unsigned int version)
984  {
985  ar & BOOST_SERIALIZATION_NVP(m_Width);
986  ar & BOOST_SERIALIZATION_NVP(m_Height);
987  }
988  }; // Size2<T>
989 
993 
997  template<typename T>
998  class Vector2
999  {
1000  public:
1005  {
1006  m_Values[0] = 0;
1007  m_Values[1] = 0;
1008  }
1009 
1015  Vector2(T x, T y)
1016  {
1017  m_Values[0] = x;
1018  m_Values[1] = y;
1019  }
1020 
1021  public:
1026  inline const T& GetX() const
1027  {
1028  return m_Values[0];
1029  }
1030 
1035  inline void SetX(const T& x)
1036  {
1037  m_Values[0] = x;
1038  }
1039 
1044  inline const T& GetY() const
1045  {
1046  return m_Values[1];
1047  }
1048 
1053  inline void SetY(const T& y)
1054  {
1055  m_Values[1] = y;
1056  }
1057 
1062  inline void MakeFloor(const Vector2& rOther)
1063  {
1064  if ( rOther.m_Values[0] < m_Values[0] ) m_Values[0] = rOther.m_Values[0];
1065  if ( rOther.m_Values[1] < m_Values[1] ) m_Values[1] = rOther.m_Values[1];
1066  }
1067 
1072  inline void MakeCeil(const Vector2& rOther)
1073  {
1074  if ( rOther.m_Values[0] > m_Values[0] ) m_Values[0] = rOther.m_Values[0];
1075  if ( rOther.m_Values[1] > m_Values[1] ) m_Values[1] = rOther.m_Values[1];
1076  }
1077 
1082  inline kt_double SquaredLength() const
1083  {
1084  return math::Square(m_Values[0]) + math::Square(m_Values[1]);
1085  }
1086 
1091  inline kt_double Length() const
1092  {
1093  return sqrt(SquaredLength());
1094  }
1095 
1100  inline kt_double SquaredDistance(const Vector2& rOther) const
1101  {
1102  return (*this - rOther).SquaredLength();
1103  }
1104 
1110  inline kt_double Distance(const Vector2& rOther) const
1111  {
1112  return sqrt(SquaredDistance(rOther));
1113  }
1114 
1115  public:
1119  inline void operator += (const Vector2& rOther)
1120  {
1121  m_Values[0] += rOther.m_Values[0];
1122  m_Values[1] += rOther.m_Values[1];
1123  }
1124 
1128  inline void operator -= (const Vector2& rOther)
1129  {
1130  m_Values[0] -= rOther.m_Values[0];
1131  m_Values[1] -= rOther.m_Values[1];
1132  }
1133 
1139  inline const Vector2 operator + (const Vector2& rOther) const
1140  {
1141  return Vector2(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1]);
1142  }
1143 
1149  inline const Vector2 operator - (const Vector2& rOther) const
1150  {
1151  return Vector2(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1]);
1152  }
1153 
1158  inline void operator /= (T scalar)
1159  {
1160  m_Values[0] /= scalar;
1161  m_Values[1] /= scalar;
1162  }
1163 
1169  inline const Vector2 operator / (T scalar) const
1170  {
1171  return Vector2(m_Values[0] / scalar, m_Values[1] / scalar);
1172  }
1173 
1179  inline kt_double operator * (const Vector2& rOther) const
1180  {
1181  return m_Values[0] * rOther.m_Values[0] + m_Values[1] * rOther.m_Values[1];
1182  }
1183 
1188  inline const Vector2 operator * (T scalar) const
1189  {
1190  return Vector2(m_Values[0] * scalar, m_Values[1] * scalar);
1191  }
1192 
1197  inline const Vector2 operator - (T scalar) const
1198  {
1199  return Vector2(m_Values[0] - scalar, m_Values[1] - scalar);
1200  }
1201 
1206  inline void operator *= (T scalar)
1207  {
1208  m_Values[0] *= scalar;
1209  m_Values[1] *= scalar;
1210  }
1211 
1216  inline kt_bool operator == (const Vector2& rOther) const
1217  {
1218  return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1]);
1219  }
1220 
1225  inline kt_bool operator != (const Vector2& rOther) const
1226  {
1227  return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1]);
1228  }
1229 
1235  inline kt_bool operator < (const Vector2& rOther) const
1236  {
1237  if (m_Values[0] < rOther.m_Values[0])
1238  return true;
1239  else if (m_Values[0] > rOther.m_Values[0])
1240  return false;
1241  else
1242  return (m_Values[1] < rOther.m_Values[1]);
1243  }
1244 
1250  friend inline std::ostream& operator << (std::ostream& rStream, const Vector2& rVector)
1251  {
1252  rStream << rVector.GetX() << " " << rVector.GetY();
1253  return rStream;
1254  }
1255 
1260  friend inline std::istream& operator >> (std::istream& rStream, const Vector2& /*rVector*/)
1261  {
1262  // Implement me!! TODO(lucbettaieb): What the what? Do I need to implement this?
1263  return rStream;
1264  }
1265 
1266  friend class boost::serialization::access;
1267  template<class Archive>
1268  void serialize(Archive &ar, const unsigned int version)
1269  {
1270  ar & boost::serialization::make_nvp("m_Values_0", m_Values[0]);
1271  ar & boost::serialization::make_nvp("m_Values_1", m_Values[1]);
1272  }
1273 
1274  private:
1275  T m_Values[2];
1276  }; // Vector2<T>
1277 
1281  typedef std::vector< Vector2<kt_double> > PointVectorDouble;
1282 
1286 
1290  template<typename T>
1291  class Vector3
1292  {
1293  public:
1298  {
1299  m_Values[0] = 0;
1300  m_Values[1] = 0;
1301  m_Values[2] = 0;
1302  }
1303 
1310  Vector3(T x, T y, T z)
1311  {
1312  m_Values[0] = x;
1313  m_Values[1] = y;
1314  m_Values[2] = z;
1315  }
1316 
1321  Vector3(const Vector3& rOther)
1322  {
1323  m_Values[0] = rOther.m_Values[0];
1324  m_Values[1] = rOther.m_Values[1];
1325  m_Values[2] = rOther.m_Values[2];
1326  }
1327 
1328  public:
1333  inline const T& GetX() const
1334  {
1335  return m_Values[0];
1336  }
1337 
1342  inline void SetX(const T& x)
1343  {
1344  m_Values[0] = x;
1345  }
1346 
1351  inline const T& GetY() const
1352  {
1353  return m_Values[1];
1354  }
1355 
1360  inline void SetY(const T& y)
1361  {
1362  m_Values[1] = y;
1363  }
1364 
1369  inline const T& GetZ() const
1370  {
1371  return m_Values[2];
1372  }
1373 
1378  inline void SetZ(const T& z)
1379  {
1380  m_Values[2] = z;
1381  }
1382 
1387  inline void MakeFloor(const Vector3& rOther)
1388  {
1389  if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
1390  if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
1391  if (rOther.m_Values[2] < m_Values[2]) m_Values[2] = rOther.m_Values[2];
1392  }
1393 
1398  inline void MakeCeil(const Vector3& rOther)
1399  {
1400  if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0];
1401  if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1];
1402  if (rOther.m_Values[2] > m_Values[2]) m_Values[2] = rOther.m_Values[2];
1403  }
1404 
1409  inline kt_double SquaredLength() const
1410  {
1411  return math::Square(m_Values[0]) + math::Square(m_Values[1]) + math::Square(m_Values[2]);
1412  }
1413 
1418  inline kt_double Length() const
1419  {
1420  return sqrt(SquaredLength());
1421  }
1422 
1427  inline std::string ToString() const
1428  {
1429  std::stringstream converter;
1430  converter.precision(std::numeric_limits<double>::digits10);
1431 
1432  converter << GetX() << " " << GetY() << " " << GetZ();
1433 
1434  return converter.str();
1435  }
1436 
1437  public:
1441  inline Vector3& operator = (const Vector3& rOther)
1442  {
1443  m_Values[0] = rOther.m_Values[0];
1444  m_Values[1] = rOther.m_Values[1];
1445  m_Values[2] = rOther.m_Values[2];
1446 
1447  return *this;
1448  }
1449 
1455  inline const Vector3 operator + (const Vector3& rOther) const
1456  {
1457  return Vector3(m_Values[0] + rOther.m_Values[0],
1458  m_Values[1] + rOther.m_Values[1],
1459  m_Values[2] + rOther.m_Values[2]);
1460  }
1461 
1467  inline const Vector3 operator + (kt_double scalar) const
1468  {
1469  return Vector3(m_Values[0] + scalar,
1470  m_Values[1] + scalar,
1471  m_Values[2] + scalar);
1472  }
1473 
1479  inline const Vector3 operator - (const Vector3& rOther) const
1480  {
1481  return Vector3(m_Values[0] - rOther.m_Values[0],
1482  m_Values[1] - rOther.m_Values[1],
1483  m_Values[2] - rOther.m_Values[2]);
1484  }
1485 
1491  inline const Vector3 operator - (kt_double scalar) const
1492  {
1493  return Vector3(m_Values[0] - scalar, m_Values[1] - scalar, m_Values[2] - scalar);
1494  }
1495 
1500  inline const Vector3 operator * (T scalar) const
1501  {
1502  return Vector3(m_Values[0] * scalar, m_Values[1] * scalar, m_Values[2] * scalar);
1503  }
1504 
1509  inline kt_bool operator == (const Vector3& rOther) const
1510  {
1511  return (m_Values[0] == rOther.m_Values[0] &&
1512  m_Values[1] == rOther.m_Values[1] &&
1513  m_Values[2] == rOther.m_Values[2]);
1514  }
1515 
1520  inline kt_bool operator != (const Vector3& rOther) const
1521  {
1522  return (m_Values[0] != rOther.m_Values[0] ||
1523  m_Values[1] != rOther.m_Values[1] ||
1524  m_Values[2] != rOther.m_Values[2]);
1525  }
1526 
1532  friend inline std::ostream& operator << (std::ostream& rStream, const Vector3& rVector)
1533  {
1534  rStream << rVector.ToString();
1535  return rStream;
1536  }
1537 
1542  friend inline std::istream& operator >> (std::istream& rStream, const Vector3& /*rVector*/)
1543  {
1544  // Implement me!!
1545  return rStream;
1546  }
1547 
1548  private:
1549  T m_Values[3];
1550  }; // Vector3
1551 
1555 
1578  {
1579  public:
1583  inline Quaternion()
1584  {
1585  m_Values[0] = 0.0;
1586  m_Values[1] = 0.0;
1587  m_Values[2] = 0.0;
1588  m_Values[3] = 1.0;
1589  }
1590 
1599  {
1600  m_Values[0] = x;
1601  m_Values[1] = y;
1602  m_Values[2] = z;
1603  m_Values[3] = w;
1604  }
1605 
1609  inline Quaternion(const Quaternion& rQuaternion)
1610  {
1611  m_Values[0] = rQuaternion.m_Values[0];
1612  m_Values[1] = rQuaternion.m_Values[1];
1613  m_Values[2] = rQuaternion.m_Values[2];
1614  m_Values[3] = rQuaternion.m_Values[3];
1615  }
1616 
1617  public:
1622  inline kt_double GetX() const
1623  {
1624  return m_Values[0];
1625  }
1626 
1631  inline void SetX(kt_double x)
1632  {
1633  m_Values[0] = x;
1634  }
1635 
1640  inline kt_double GetY() const
1641  {
1642  return m_Values[1];
1643  }
1644 
1649  inline void SetY(kt_double y)
1650  {
1651  m_Values[1] = y;
1652  }
1653 
1658  inline kt_double GetZ() const
1659  {
1660  return m_Values[2];
1661  }
1662 
1667  inline void SetZ(kt_double z)
1668  {
1669  m_Values[2] = z;
1670  }
1671 
1676  inline kt_double GetW() const
1677  {
1678  return m_Values[3];
1679  }
1680 
1685  inline void SetW(kt_double w)
1686  {
1687  m_Values[3] = w;
1688  }
1689 
1697  void ToEulerAngles(kt_double& rYaw, kt_double& rPitch, kt_double& rRoll) const
1698  {
1699  kt_double test = m_Values[0] * m_Values[1] + m_Values[2] * m_Values[3];
1700 
1701  if (test > 0.499)
1702  {
1703  // singularity at north pole
1704  rYaw = 2 * atan2(m_Values[0], m_Values[3]);;
1705  rPitch = KT_PI_2;
1706  rRoll = 0;
1707  }
1708  else if (test < -0.499)
1709  {
1710  // singularity at south pole
1711  rYaw = -2 * atan2(m_Values[0], m_Values[3]);
1712  rPitch = -KT_PI_2;
1713  rRoll = 0;
1714  }
1715  else
1716  {
1717  kt_double sqx = m_Values[0] * m_Values[0];
1718  kt_double sqy = m_Values[1] * m_Values[1];
1719  kt_double sqz = m_Values[2] * m_Values[2];
1720 
1721  rYaw = atan2(2 * m_Values[1] * m_Values[3] - 2 * m_Values[0] * m_Values[2], 1 - 2 * sqy - 2 * sqz);
1722  rPitch = asin(2 * test);
1723  rRoll = atan2(2 * m_Values[0] * m_Values[3] - 2 * m_Values[1] * m_Values[2], 1 - 2 * sqx - 2 * sqz);
1724  }
1725  }
1726 
1735  {
1736  kt_double angle;
1737 
1738  angle = yaw * 0.5;
1739  kt_double cYaw = cos(angle);
1740  kt_double sYaw = sin(angle);
1741 
1742  angle = pitch * 0.5;
1743  kt_double cPitch = cos(angle);
1744  kt_double sPitch = sin(angle);
1745 
1746  angle = roll * 0.5;
1747  kt_double cRoll = cos(angle);
1748  kt_double sRoll = sin(angle);
1749 
1750  m_Values[0] = sYaw * sPitch * cRoll + cYaw * cPitch * sRoll;
1751  m_Values[1] = sYaw * cPitch * cRoll + cYaw * sPitch * sRoll;
1752  m_Values[2] = cYaw * sPitch * cRoll - sYaw * cPitch * sRoll;
1753  m_Values[3] = cYaw * cPitch * cRoll - sYaw * sPitch * sRoll;
1754  }
1755 
1760  inline Quaternion& operator = (const Quaternion& rQuaternion)
1761  {
1762  m_Values[0] = rQuaternion.m_Values[0];
1763  m_Values[1] = rQuaternion.m_Values[1];
1764  m_Values[2] = rQuaternion.m_Values[2];
1765  m_Values[3] = rQuaternion.m_Values[3];
1766 
1767  return(*this);
1768  }
1769 
1774  inline kt_bool operator == (const Quaternion& rOther) const
1775  {
1776  return (m_Values[0] == rOther.m_Values[0] &&
1777  m_Values[1] == rOther.m_Values[1] &&
1778  m_Values[2] == rOther.m_Values[2] &&
1779  m_Values[3] == rOther.m_Values[3]);
1780  }
1781 
1786  inline kt_bool operator != (const Quaternion& rOther) const
1787  {
1788  return (m_Values[0] != rOther.m_Values[0] ||
1789  m_Values[1] != rOther.m_Values[1] ||
1790  m_Values[2] != rOther.m_Values[2] ||
1791  m_Values[3] != rOther.m_Values[3]);
1792  }
1793 
1799  friend inline std::ostream& operator << (std::ostream& rStream, const Quaternion& rQuaternion)
1800  {
1801  rStream << rQuaternion.m_Values[0] << " "
1802  << rQuaternion.m_Values[1] << " "
1803  << rQuaternion.m_Values[2] << " "
1804  << rQuaternion.m_Values[3];
1805  return rStream;
1806  }
1807 
1808  private:
1809  kt_double m_Values[4];
1810  };
1811 
1815 
1820  template<typename T>
1822  {
1823  public:
1828  {
1829  }
1830 
1838  Rectangle2(T x, T y, T width, T height)
1839  : m_Position(x, y)
1840  , m_Size(width, height)
1841  {
1842  }
1843 
1849  Rectangle2(const Vector2<T>& rPosition, const Size2<T>& rSize)
1850  : m_Position(rPosition)
1851  , m_Size(rSize)
1852  {
1853  }
1854 
1858  Rectangle2(const Rectangle2& rOther)
1859  : m_Position(rOther.m_Position)
1860  , m_Size(rOther.m_Size)
1861  {
1862  }
1863 
1864  public:
1869  inline T GetX() const
1870  {
1871  return m_Position.GetX();
1872  }
1873 
1878  inline void SetX(T x)
1879  {
1880  m_Position.SetX(x);
1881  }
1882 
1887  inline T GetY() const
1888  {
1889  return m_Position.GetY();
1890  }
1891 
1896  inline void SetY(T y)
1897  {
1898  m_Position.SetY(y);
1899  }
1900 
1905  inline T GetWidth() const
1906  {
1907  return m_Size.GetWidth();
1908  }
1909 
1914  inline void SetWidth(T width)
1915  {
1916  m_Size.SetWidth(width);
1917  }
1918 
1923  inline T GetHeight() const
1924  {
1925  return m_Size.GetHeight();
1926  }
1927 
1932  inline void SetHeight(T height)
1933  {
1934  m_Size.SetHeight(height);
1935  }
1936 
1941  inline const Vector2<T>& GetPosition() const
1942  {
1943  return m_Position;
1944  }
1945 
1951  inline void SetPosition(const T& rX, const T& rY)
1952  {
1953  m_Position = Vector2<T>(rX, rY);
1954  }
1955 
1960  inline void SetPosition(const Vector2<T>& rPosition)
1961  {
1962  m_Position = rPosition;
1963  }
1964 
1969  inline const Size2<T>& GetSize() const
1970  {
1971  return m_Size;
1972  }
1973 
1978  inline void SetSize(const Size2<T>& rSize)
1979  {
1980  m_Size = rSize;
1981  }
1982 
1987  inline const Vector2<T> GetCenter() const
1988  {
1989  return Vector2<T>(m_Position.GetX() + m_Size.GetWidth() * 0.5, m_Position.GetY() + m_Size.GetHeight() * 0.5);
1990  }
1991 
1992  public:
1996  Rectangle2& operator = (const Rectangle2& rOther)
1997  {
1998  m_Position = rOther.m_Position;
1999  m_Size = rOther.m_Size;
2000 
2001  return *this;
2002  }
2003 
2007  inline kt_bool operator == (const Rectangle2& rOther) const
2008  {
2009  return (m_Position == rOther.m_Position && m_Size == rOther.m_Size);
2010  }
2011 
2015  inline kt_bool operator != (const Rectangle2& rOther) const
2016  {
2017  return (m_Position != rOther.m_Position || m_Size != rOther.m_Size);
2018  }
2019 
2020  private:
2026  friend class boost::serialization::access;
2027  template<class Archive>
2028  void serialize(Archive &ar, const unsigned int version)
2029  {
2030  ar & BOOST_SERIALIZATION_NVP(m_Position);
2031  ar & BOOST_SERIALIZATION_NVP(m_Size);
2032  }
2033  }; // Rectangle2
2034 
2038 
2039  class Pose3;
2040 
2044  class Pose2
2045  {
2046  public:
2051  : m_Heading(0.0)
2052  {
2053  }
2054 
2060  Pose2(const Vector2<kt_double>& rPosition, kt_double heading)
2061  : m_Position(rPosition)
2062  , m_Heading(heading)
2063  {
2064  }
2065 
2073  : m_Position(x, y)
2074  , m_Heading(heading)
2075  {
2076  }
2077 
2081  Pose2(const Pose3& rPose);
2082 
2086  Pose2(const Pose2& rOther)
2087  : m_Position(rOther.m_Position)
2088  , m_Heading(rOther.m_Heading)
2089  {
2090  }
2091 
2092  public:
2097  inline kt_double GetX() const
2098  {
2099  return m_Position.GetX();
2100  }
2101 
2106  inline void SetX(kt_double x)
2107  {
2108  m_Position.SetX(x);
2109  }
2110 
2115  inline kt_double GetY() const
2116  {
2117  return m_Position.GetY();
2118  }
2119 
2124  inline void SetY(kt_double y)
2125  {
2126  m_Position.SetY(y);
2127  }
2128 
2133  inline const Vector2<kt_double>& GetPosition() const
2134  {
2135  return m_Position;
2136  }
2137 
2142  inline void SetPosition(const Vector2<kt_double>& rPosition)
2143  {
2144  m_Position = rPosition;
2145  }
2146 
2151  inline kt_double GetHeading() const
2152  {
2153  return m_Heading;
2154  }
2155 
2160  inline void SetHeading(kt_double heading)
2161  {
2162  m_Heading = heading;
2163  }
2164 
2169  inline kt_double SquaredDistance(const Pose2& rOther) const
2170  {
2171  return m_Position.SquaredDistance(rOther.m_Position);
2172  }
2173 
2174  public:
2178  inline Pose2& operator = (const Pose2& rOther)
2179  {
2180  m_Position = rOther.m_Position;
2181  m_Heading = rOther.m_Heading;
2182 
2183  return *this;
2184  }
2185 
2189  inline kt_bool operator == (const Pose2& rOther) const
2190  {
2191  return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading);
2192  }
2193 
2197  inline kt_bool operator != (const Pose2& rOther) const
2198  {
2199  return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading);
2200  }
2201 
2205  inline void operator += (const Pose2& rOther)
2206  {
2207  m_Position += rOther.m_Position;
2208  m_Heading = math::NormalizeAngle(m_Heading + rOther.m_Heading);
2209  }
2210 
2216  inline Pose2 operator + (const Pose2& rOther) const
2217  {
2218  return Pose2(m_Position + rOther.m_Position, math::NormalizeAngle(m_Heading + rOther.m_Heading));
2219  }
2220 
2226  inline Pose2 operator - (const Pose2& rOther) const
2227  {
2228  return Pose2(m_Position - rOther.m_Position, math::NormalizeAngle(m_Heading - rOther.m_Heading));
2229  }
2230 
2235  friend inline std::istream& operator >> (std::istream& rStream, const Pose2& /*rPose*/)
2236  {
2237  // Implement me!!
2238  return rStream;
2239  }
2240 
2246  friend inline std::ostream& operator << (std::ostream& rStream, const Pose2& rPose)
2247  {
2248  rStream << rPose.m_Position.GetX() << " " << rPose.m_Position.GetY() << " " << rPose.m_Heading;
2249  return rStream;
2250  }
2251 
2252  friend class boost::serialization::access;
2253  template<class Archive>
2254  void serialize(Archive &ar, const unsigned int version)
2255  {
2256  ar & BOOST_SERIALIZATION_NVP(m_Position);
2257  ar & BOOST_SERIALIZATION_NVP(m_Heading);
2258  }
2259 
2260  private:
2262 
2264  }; // Pose2
2265 
2269  typedef std::vector< Pose2 > Pose2Vector;
2270 
2274 
2282  class Pose3
2283  {
2284  public:
2289  {
2290  }
2291 
2296  Pose3(const Vector3<kt_double>& rPosition)
2297  : m_Position(rPosition)
2298  {
2299  }
2300 
2306  Pose3(const Vector3<kt_double>& rPosition, const karto::Quaternion& rOrientation)
2307  : m_Position(rPosition)
2308  , m_Orientation(rOrientation)
2309  {
2310  }
2311 
2315  Pose3(const Pose3& rOther)
2316  : m_Position(rOther.m_Position)
2317  , m_Orientation(rOther.m_Orientation)
2318  {
2319  }
2320 
2324  Pose3(const Pose2& rPose)
2325  {
2326  m_Position = Vector3<kt_double>(rPose.GetX(), rPose.GetY(), 0.0);
2327  m_Orientation.FromEulerAngles(rPose.GetHeading(), 0.0, 0.0);
2328  }
2329 
2330  public:
2335  inline const Vector3<kt_double>& GetPosition() const
2336  {
2337  return m_Position;
2338  }
2339 
2344  inline void SetPosition(const Vector3<kt_double>& rPosition)
2345  {
2346  m_Position = rPosition;
2347  }
2348 
2353  inline const Quaternion& GetOrientation() const
2354  {
2355  return m_Orientation;
2356  }
2357 
2362  inline void SetOrientation(const Quaternion& rOrientation)
2363  {
2364  m_Orientation = rOrientation;
2365  }
2366 
2371  inline std::string ToString()
2372  {
2373  std::stringstream converter;
2374  converter.precision(std::numeric_limits<double>::digits10);
2375 
2376  converter << GetPosition() << " " << GetOrientation();
2377 
2378  return converter.str();
2379  }
2380 
2381  public:
2385  inline Pose3& operator = (const Pose3& rOther)
2386  {
2387  m_Position = rOther.m_Position;
2388  m_Orientation = rOther.m_Orientation;
2389 
2390  return *this;
2391  }
2392 
2396  inline kt_bool operator == (const Pose3& rOther) const
2397  {
2398  return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation);
2399  }
2400 
2404  inline kt_bool operator != (const Pose3& rOther) const
2405  {
2406  return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation);
2407  }
2408 
2414  friend inline std::ostream& operator << (std::ostream& rStream, const Pose3& rPose)
2415  {
2416  rStream << rPose.GetPosition() << ", " << rPose.GetOrientation();
2417  return rStream;
2418  }
2419 
2424  friend inline std::istream& operator >> (std::istream& rStream, const Pose3& /*rPose*/)
2425  {
2426  // Implement me!!
2427  return rStream;
2428  }
2429 
2430  private:
2433  }; // Pose3
2434 
2438 
2442  class Matrix3
2443  {
2444  public:
2449  {
2450  Clear();
2451  }
2452 
2456  inline Matrix3(const Matrix3& rOther)
2457  {
2458  memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
2459  }
2460 
2461  public:
2466  {
2467  memset(m_Matrix, 0, 9*sizeof(kt_double));
2468 
2469  for (kt_int32s i = 0; i < 3; i++)
2470  {
2471  m_Matrix[i][i] = 1.0;
2472  }
2473  }
2474 
2478  void Clear()
2479  {
2480  memset(m_Matrix, 0, 9*sizeof(kt_double));
2481  }
2482 
2490  void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
2491  {
2492  kt_double cosRadians = cos(radians);
2493  kt_double sinRadians = sin(radians);
2494  kt_double oneMinusCos = 1.0 - cosRadians;
2495 
2496  kt_double xx = x * x;
2497  kt_double yy = y * y;
2498  kt_double zz = z * z;
2499 
2500  kt_double xyMCos = x * y * oneMinusCos;
2501  kt_double xzMCos = x * z * oneMinusCos;
2502  kt_double yzMCos = y * z * oneMinusCos;
2503 
2504  kt_double xSin = x * sinRadians;
2505  kt_double ySin = y * sinRadians;
2506  kt_double zSin = z * sinRadians;
2507 
2508  m_Matrix[0][0] = xx * oneMinusCos + cosRadians;
2509  m_Matrix[0][1] = xyMCos - zSin;
2510  m_Matrix[0][2] = xzMCos + ySin;
2511 
2512  m_Matrix[1][0] = xyMCos + zSin;
2513  m_Matrix[1][1] = yy * oneMinusCos + cosRadians;
2514  m_Matrix[1][2] = yzMCos - xSin;
2515 
2516  m_Matrix[2][0] = xzMCos - ySin;
2517  m_Matrix[2][1] = yzMCos + xSin;
2518  m_Matrix[2][2] = zz * oneMinusCos + cosRadians;
2519  }
2520 
2526  {
2527  Matrix3 transpose;
2528 
2529  for (kt_int32u row = 0; row < 3; row++)
2530  {
2531  for (kt_int32u col = 0; col < 3; col++)
2532  {
2533  transpose.m_Matrix[row][col] = m_Matrix[col][row];
2534  }
2535  }
2536 
2537  return transpose;
2538  }
2539 
2544  {
2545  Matrix3 kInverse = *this;
2546  kt_bool haveInverse = InverseFast(kInverse, 1e-14);
2547  if (haveInverse == false)
2548  {
2549  assert(false);
2550  }
2551  return kInverse;
2552  }
2553 
2558  kt_bool InverseFast(Matrix3& rkInverse, kt_double fTolerance = KT_TOLERANCE) const
2559  {
2560  // Invert a 3x3 using cofactors. This is about 8 times faster than
2561  // the Numerical Recipes code which uses Gaussian elimination.
2562  rkInverse.m_Matrix[0][0] = m_Matrix[1][1]*m_Matrix[2][2] - m_Matrix[1][2]*m_Matrix[2][1];
2563  rkInverse.m_Matrix[0][1] = m_Matrix[0][2]*m_Matrix[2][1] - m_Matrix[0][1]*m_Matrix[2][2];
2564  rkInverse.m_Matrix[0][2] = m_Matrix[0][1]*m_Matrix[1][2] - m_Matrix[0][2]*m_Matrix[1][1];
2565  rkInverse.m_Matrix[1][0] = m_Matrix[1][2]*m_Matrix[2][0] - m_Matrix[1][0]*m_Matrix[2][2];
2566  rkInverse.m_Matrix[1][1] = m_Matrix[0][0]*m_Matrix[2][2] - m_Matrix[0][2]*m_Matrix[2][0];
2567  rkInverse.m_Matrix[1][2] = m_Matrix[0][2]*m_Matrix[1][0] - m_Matrix[0][0]*m_Matrix[1][2];
2568  rkInverse.m_Matrix[2][0] = m_Matrix[1][0]*m_Matrix[2][1] - m_Matrix[1][1]*m_Matrix[2][0];
2569  rkInverse.m_Matrix[2][1] = m_Matrix[0][1]*m_Matrix[2][0] - m_Matrix[0][0]*m_Matrix[2][1];
2570  rkInverse.m_Matrix[2][2] = m_Matrix[0][0]*m_Matrix[1][1] - m_Matrix[0][1]*m_Matrix[1][0];
2571 
2572  kt_double fDet = m_Matrix[0][0]*rkInverse.m_Matrix[0][0] +
2573  m_Matrix[0][1]*rkInverse.m_Matrix[1][0] +
2574  m_Matrix[0][2]*rkInverse.m_Matrix[2][0];
2575 
2576  if (fabs(fDet) <= fTolerance)
2577  {
2578  return false;
2579  }
2580 
2581  kt_double fInvDet = 1.0/fDet;
2582  for (size_t row = 0; row < 3; row++)
2583  {
2584  for (size_t col = 0; col < 3; col++)
2585  {
2586  rkInverse.m_Matrix[row][col] *= fInvDet;
2587  }
2588  }
2589 
2590  return true;
2591  }
2592 
2597  inline std::string ToString() const
2598  {
2599  std::stringstream converter;
2600  converter.precision(std::numeric_limits<double>::digits10);
2601 
2602  for (int row = 0; row < 3; row++)
2603  {
2604  for (int col = 0; col < 3; col++)
2605  {
2606  converter << m_Matrix[row][col] << " ";
2607  }
2608  }
2609 
2610  return converter.str();
2611  }
2612 
2613  public:
2617  inline Matrix3& operator = (const Matrix3& rOther)
2618  {
2619  memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
2620  return *this;
2621  }
2622 
2630  {
2631  return m_Matrix[row][column];
2632  }
2633 
2640  inline kt_double operator()(kt_int32u row, kt_int32u column) const
2641  {
2642  return m_Matrix[row][column];
2643  }
2644 
2650  Matrix3 operator * (const Matrix3& rOther) const
2651  {
2652  Matrix3 product;
2653 
2654  for (size_t row = 0; row < 3; row++)
2655  {
2656  for (size_t col = 0; col < 3; col++)
2657  {
2658  product.m_Matrix[row][col] = m_Matrix[row][0]*rOther.m_Matrix[0][col] +
2659  m_Matrix[row][1]*rOther.m_Matrix[1][col] +
2660  m_Matrix[row][2]*rOther.m_Matrix[2][col];
2661  }
2662  }
2663 
2664  return product;
2665  }
2666 
2672  inline Pose2 operator * (const Pose2& rPose2) const
2673  {
2674  Pose2 pose2;
2675 
2676  pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] *
2677  rPose2.GetY() + m_Matrix[0][2] * rPose2.GetHeading());
2678  pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] *
2679  rPose2.GetY() + m_Matrix[1][2] * rPose2.GetHeading());
2680  pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() + m_Matrix[2][1] *
2681  rPose2.GetY() + m_Matrix[2][2] * rPose2.GetHeading());
2682 
2683  return pose2;
2684  }
2685 
2690  inline void operator += (const Matrix3& rkMatrix)
2691  {
2692  for (kt_int32u row = 0; row < 3; row++)
2693  {
2694  for (kt_int32u col = 0; col < 3; col++)
2695  {
2696  m_Matrix[row][col] += rkMatrix.m_Matrix[row][col];
2697  }
2698  }
2699  }
2700 
2706  friend inline std::ostream& operator << (std::ostream& rStream, const Matrix3& rMatrix)
2707  {
2708  rStream << rMatrix.ToString();
2709  return rStream;
2710  }
2711 
2712  private:
2713  kt_double m_Matrix[3][3];
2714  friend class boost::serialization::access;
2715  template<class Archive>
2716  void serialize(Archive &ar, const unsigned int version)
2717  {
2718  ar & BOOST_SERIALIZATION_NVP(m_Matrix);
2719  }
2720  }; // Matrix3
2721 
2725 
2729  class Matrix
2730  {
2731  public:
2735  Matrix(kt_int32u rows, kt_int32u columns)
2736  : m_Rows(rows)
2737  , m_Columns(columns)
2738  , m_pData(NULL)
2739  {
2740  Allocate();
2741 
2742  Clear();
2743  }
2744 
2748  virtual ~Matrix()
2749  {
2750  delete [] m_pData;
2751  }
2752 
2753  public:
2757  void Clear()
2758  {
2759  if (m_pData != NULL)
2760  {
2761  memset(m_pData, 0, sizeof(kt_double) * m_Rows * m_Columns);
2762  }
2763  }
2764 
2769  inline kt_int32u GetRows() const
2770  {
2771  return m_Rows;
2772  }
2773 
2778  inline kt_int32u GetColumns() const
2779  {
2780  return m_Columns;
2781  }
2782 
2790  {
2791  RangeCheck(row, column);
2792 
2793  return m_pData[row + column * m_Rows];
2794  }
2795 
2802  inline const kt_double& operator()(kt_int32u row, kt_int32u column) const
2803  {
2804  RangeCheck(row, column);
2805 
2806  return m_pData[row + column * m_Rows];
2807  }
2808 
2809  private:
2813  void Allocate()
2814  {
2815  try
2816  {
2817  if (m_pData != NULL)
2818  {
2819  delete[] m_pData;
2820  }
2821 
2822  m_pData = new kt_double[m_Rows * m_Columns];
2823  }
2824  catch (const std::bad_alloc& ex)
2825  {
2826  throw Exception("Matrix allocation error");
2827  }
2828 
2829  if (m_pData == NULL)
2830  {
2831  throw Exception("Matrix allocation error");
2832  }
2833  }
2834 
2840  inline void RangeCheck(kt_int32u row, kt_int32u column) const
2841  {
2842  if (math::IsUpTo(row, m_Rows) == false)
2843  {
2844  throw Exception("Matrix - RangeCheck ERROR!!!!");
2845  }
2846 
2847  if (math::IsUpTo(column, m_Columns) == false)
2848  {
2849  throw Exception("Matrix - RangeCheck ERROR!!!!");
2850  }
2851  }
2852 
2853  private:
2856 
2858  }; // Matrix
2859 
2863 
2868  {
2869  public:
2870  /*
2871  * Default constructor
2872  */
2874  : m_Minimum(999999999999999999.99999, 999999999999999999.99999)
2875  , m_Maximum(-999999999999999999.99999, -999999999999999999.99999)
2876  {
2877  }
2878 
2879  public:
2883  inline const Vector2<kt_double>& GetMinimum() const
2884  {
2885  return m_Minimum;
2886  }
2887 
2891  inline void SetMinimum(const Vector2<kt_double>& mMinimum)
2892  {
2893  m_Minimum = mMinimum;
2894  }
2895 
2899  inline const Vector2<kt_double>& GetMaximum() const
2900  {
2901  return m_Maximum;
2902  }
2903 
2907  inline void SetMaximum(const Vector2<kt_double>& rMaximum)
2908  {
2909  m_Maximum = rMaximum;
2910  }
2911 
2915  inline Size2<kt_double> GetSize() const
2916  {
2917  Vector2<kt_double> size = m_Maximum - m_Minimum;
2918 
2919  return Size2<kt_double>(size.GetX(), size.GetY());
2920  }
2921 
2925  inline void Add(const Vector2<kt_double>& rPoint)
2926  {
2927  m_Minimum.MakeFloor(rPoint);
2928  m_Maximum.MakeCeil(rPoint);
2929  }
2930 
2934  inline void Add(const BoundingBox2& rBoundingBox)
2935  {
2936  Add(rBoundingBox.GetMinimum());
2937  Add(rBoundingBox.GetMaximum());
2938  }
2939 
2945  inline kt_bool IsInBounds(const Vector2<kt_double>& rPoint) const
2946  {
2947  return (math::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) &&
2948  math::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY()));
2949  }
2950 
2954  friend class boost::serialization::access;
2955  template<class Archive>
2956  void serialize(Archive &ar, const unsigned int version)
2957  {
2958  ar & BOOST_SERIALIZATION_NVP(m_Minimum);
2959  ar & BOOST_SERIALIZATION_NVP(m_Maximum);
2960  }
2961 
2962  private:
2965  }; // BoundingBox2
2966 
2970 
2975  {
2976  public:
2981  Transform(const Pose2& rPose)
2982  {
2983  SetTransform(Pose2(), rPose);
2984  }
2985 
2991  Transform(const Pose2& rPose1, const Pose2& rPose2)
2992  {
2993  SetTransform(rPose1, rPose2);
2994  }
2995 
2996  public:
3002  inline Pose2 TransformPose(const Pose2& rSourcePose)
3003  {
3004  Pose2 newPosition = m_Transform + m_Rotation * rSourcePose;
3005  kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() + m_Transform.GetHeading());
3006 
3007  return Pose2(newPosition.GetPosition(), angle);
3008  }
3009 
3015  inline Pose2 InverseTransformPose(const Pose2& rSourcePose)
3016  {
3017  Pose2 newPosition = m_InverseRotation * (rSourcePose - m_Transform);
3018  kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() - m_Transform.GetHeading());
3019 
3020  // components of transform
3021  return Pose2(newPosition.GetPosition(), angle);
3022  }
3023 
3024  private:
3030  void SetTransform(const Pose2& rPose1, const Pose2& rPose2)
3031  {
3032  if (rPose1 == rPose2)
3033  {
3034  m_Rotation.SetToIdentity();
3035  m_InverseRotation.SetToIdentity();
3036  m_Transform = Pose2();
3037  return;
3038  }
3039 
3040  // heading transformation
3041  m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading());
3042  m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading());
3043 
3044  // position transformation
3045  Pose2 newPosition;
3046  if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0)
3047  {
3048  newPosition = rPose2 - m_Rotation * rPose1;
3049  }
3050  else
3051  {
3052  newPosition = rPose2;
3053  }
3054 
3055  m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading());
3056  }
3057 
3058  private:
3059  // pose transformation
3061 
3064 
3065  friend class boost::serialization::access;
3066  template<class Archive>
3067  void serialize(Archive &ar, const unsigned int version)
3068  {
3069  ar & BOOST_SERIALIZATION_NVP(m_Transform);
3070  ar & BOOST_SERIALIZATION_NVP(m_Rotation);
3071  ar & BOOST_SERIALIZATION_NVP(m_InverseRotation);
3072  }
3073  }; // Transform
3074 
3078 
3082  typedef enum
3083  {
3085 
3089 
3093 
3097 
3102  {
3103 
3104  public:
3106  {
3107  }
3113  AbstractParameter(const std::string& rName, ParameterManager* pParameterManger = NULL)
3114  : m_Name(rName)
3115  {
3116  // if parameter manager is provided add myself to it!
3117  if (pParameterManger != NULL)
3118  {
3119  pParameterManger->Add(this);
3120  }
3121  }
3122 
3129  AbstractParameter(const std::string& rName,
3130  const std::string& rDescription,
3131  ParameterManager* pParameterManger = NULL)
3132  : m_Name(rName)
3133  , m_Description(rDescription)
3134  {
3135  // if parameter manager is provided add myself to it!
3136  if (pParameterManger != NULL)
3137  {
3138  pParameterManger->Add(this);
3139  }
3140  }
3141 
3146  {
3147  }
3148 
3149  public:
3154  inline const std::string& GetName() const
3155  {
3156  return m_Name;
3157  }
3158 
3163  inline const std::string& GetDescription() const
3164  {
3165  return m_Description;
3166  }
3167 
3172  virtual const std::string GetValueAsString() const = 0;
3173 
3178  virtual void SetValueFromString(const std::string& rStringValue) = 0;
3179 
3184  virtual AbstractParameter* Clone() = 0;
3185 
3186  public:
3192  friend std::ostream& operator << (std::ostream& rStream, const AbstractParameter& rParameter)
3193  {
3194  rStream.precision(6);
3195  rStream.flags(std::ios::fixed);
3196 
3197  rStream << rParameter.GetName() << " = " << rParameter.GetValueAsString();
3198  return rStream;
3199  }
3200 
3201  private:
3202  std::string m_Name;
3203  std::string m_Description;
3207  friend class boost::serialization::access;
3208  template<class Archive>
3209  void serialize(Archive &ar, const unsigned int version)
3210  {
3211  ar & BOOST_SERIALIZATION_NVP(m_Name);
3212  ar & BOOST_SERIALIZATION_NVP(m_Description);
3213  }
3214  }; // AbstractParameter
3215  BOOST_SERIALIZATION_ASSUME_ABSTRACT(AbstractParameter)
3216 
3217 
3218 
3224  template<typename T>
3226  {
3227  public:
3235  {
3236  }
3237  Parameter(const std::string& rName, T value, ParameterManager* pParameterManger = NULL)
3238  : AbstractParameter(rName, pParameterManger)
3239  , m_Value(value)
3240  {
3241  }
3242 
3250  Parameter(const std::string& rName,
3251  const std::string& rDescription,
3252  T value,
3253  ParameterManager* pParameterManger = NULL)
3254  : AbstractParameter(rName, rDescription, pParameterManger)
3255  , m_Value(value)
3256  {
3257  }
3258 
3262  virtual ~Parameter()
3263  {
3264  }
3265 
3266  public:
3271  inline const T& GetValue() const
3272  {
3273  return m_Value;
3274  }
3275 
3280  inline void SetValue(const T& rValue)
3281  {
3282  m_Value = rValue;
3283  }
3284 
3289  virtual const std::string GetValueAsString() const
3290  {
3291  std::stringstream converter;
3292  converter << m_Value;
3293  return converter.str();
3294  }
3295 
3300  virtual void SetValueFromString(const std::string& rStringValue)
3301  {
3302  std::stringstream converter;
3303  converter.str(rStringValue);
3304  converter >> m_Value;
3305  }
3306 
3311  virtual Parameter* Clone()
3312  {
3313  return new Parameter(GetName(), GetDescription(), GetValue());
3314  }
3315 
3316  public:
3320  Parameter& operator = (const Parameter& rOther)
3321  {
3322  m_Value = rOther.m_Value;
3323 
3324  return *this;
3325  }
3326 
3330  T operator = (T value)
3331  {
3332  m_Value = value;
3333 
3334  return m_Value;
3335  }
3336 
3340  friend class boost::serialization::access;
3341  template<class Archive>
3342  void serialize(Archive &ar, const unsigned int version)
3343  {
3344  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(AbstractParameter);
3345  ar & BOOST_SERIALIZATION_NVP(m_Value);
3346  }
3347 
3348  protected:
3353  }; // Parameter
3354  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Parameter)
3355 
3356  template<>
3357  inline void Parameter<kt_double>::SetValueFromString(const std::string& rStringValue)
3358  {
3359  int precision = std::numeric_limits<double>::digits10;
3360  std::stringstream converter;
3361  converter.precision(precision);
3362 
3363  converter.str(rStringValue);
3364 
3365  m_Value = 0.0;
3366  converter >> m_Value;
3367  }
3368 
3369  template<>
3370  inline const std::string Parameter<kt_double>::GetValueAsString() const
3371  {
3372  std::stringstream converter;
3373  converter.precision(std::numeric_limits<double>::digits10);
3374  converter << m_Value;
3375  return converter.str();
3376  }
3377 
3378  template<>
3379  inline void Parameter<kt_bool>::SetValueFromString(const std::string& rStringValue)
3380  {
3381  if (rStringValue == "true" || rStringValue == "TRUE")
3382  {
3383  m_Value = true;
3384  }
3385  else
3386  {
3387  m_Value = false;
3388  }
3389  }
3390 
3391  template<>
3392  inline const std::string Parameter<kt_bool>::GetValueAsString() const
3393  {
3394  if (m_Value == true)
3395  {
3396  return "true";
3397  }
3398 
3399  return "false";
3400  }
3401 
3405 
3409  class ParameterEnum : public Parameter<kt_int32s>
3410  {
3411  typedef std::map<std::string, kt_int32s> EnumMap;
3412 
3413  public:
3420  ParameterEnum(const std::string& rName, kt_int32s value, ParameterManager* pParameterManger = NULL)
3421  : Parameter<kt_int32s>(rName, value, pParameterManger)
3422  {
3423  }
3425  {
3426  }
3427 
3431  virtual ~ParameterEnum()
3432  {
3433  }
3434 
3435  public:
3441  {
3442  ParameterEnum* pEnum = new ParameterEnum(GetName(), GetValue());
3443 
3444  pEnum->m_EnumDefines = m_EnumDefines;
3445 
3446  return pEnum;
3447  }
3448 
3453  virtual void SetValueFromString(const std::string& rStringValue)
3454  {
3455  if (m_EnumDefines.find(rStringValue) != m_EnumDefines.end())
3456  {
3457  m_Value = m_EnumDefines[rStringValue];
3458  }
3459  else
3460  {
3461  std::string validValues;
3462 
3463  const_forEach(EnumMap, &m_EnumDefines)
3464  {
3465  validValues += iter->first + ", ";
3466  }
3467 
3468  throw Exception("Unable to set enum: " + rStringValue + ". Valid values are: " + validValues);
3469  }
3470  }
3471 
3476  virtual const std::string GetValueAsString() const
3477  {
3478  const_forEach(EnumMap, &m_EnumDefines)
3479  {
3480  if (iter->second == m_Value)
3481  {
3482  return iter->first;
3483  }
3484  }
3485 
3486  throw Exception("Unable to lookup enum");
3487  }
3488 
3494  void DefineEnumValue(kt_int32s value, const std::string& rName)
3495  {
3496  if (m_EnumDefines.find(rName) == m_EnumDefines.end())
3497  {
3498  m_EnumDefines[rName] = value;
3499  }
3500  else
3501  {
3502  std::cerr << "Overriding enum value: " << m_EnumDefines[rName] << " with " << value << std::endl;
3503 
3504  m_EnumDefines[rName] = value;
3505 
3506  assert(false);
3507  }
3508  }
3509 
3510  public:
3514  ParameterEnum& operator = (const ParameterEnum& rOther)
3515  {
3516  SetValue(rOther.GetValue());
3517 
3518  return *this;
3519  }
3520 
3524  kt_int32s operator = (kt_int32s value)
3525  {
3526  SetValue(value);
3527 
3528  return m_Value;
3529  }
3530 
3531  private:
3532  EnumMap m_EnumDefines;
3533 
3534  friend class boost::serialization::access;
3535  template<class Archive>
3536  void serialize(Archive &ar, const unsigned int version)
3537  {
3538  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Parameter<kt_int32s>);
3539  ar & BOOST_SERIALIZATION_NVP(m_EnumDefines);
3540  }
3541  }; // ParameterEnum
3542  BOOST_SERIALIZATION_ASSUME_ABSTRACT(ParameterEnum)
3546 
3550  class Parameters : public Object
3551  {
3552  public:
3553  // @cond EXCLUDE
3555  // @endcond
3556 
3557  public:
3563  {
3564  }
3565  Parameters(const std::string& rName)
3566  : Object(rName)
3567  {
3568  }
3569 
3573  virtual ~Parameters()
3574  {
3575  }
3576 
3577  friend class boost::serialization::access;
3578  template<class Archive>
3579  void serialize(Archive &ar, const unsigned int version)
3580  {
3581  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
3582  }
3583 
3584  private:
3585  Parameters(const Parameters&);
3586  const Parameters& operator=(const Parameters&);
3587  }; // Parameters
3588 
3592 
3593  class SensorData;
3594 
3598  class KARTO_EXPORT Sensor : public Object
3599  {
3600 
3604  public:
3606  {
3607  }
3608  // @cond EXCLUDE
3610  // @endcond
3611 
3612  protected:
3617  Sensor(const Name& rName);
3618 
3619  public:
3623  virtual ~Sensor();
3624 
3625  public:
3630  inline const Pose2& GetOffsetPose() const
3631  {
3632  return m_pOffsetPose->GetValue();
3633  }
3634 
3639  inline void SetOffsetPose(const Pose2& rPose)
3640  {
3641  m_pOffsetPose->SetValue(rPose);
3642  }
3643 
3648  virtual kt_bool Validate() = 0;
3649 
3655  virtual kt_bool Validate(SensorData* pSensorData) = 0;
3656 
3657  private:
3661  Sensor(const Sensor&);
3662 
3666  const Sensor& operator=(const Sensor&);
3667 
3668  private:
3673  friend class boost::serialization::access;
3674  template<class Archive>
3675  void serialize(Archive &ar, const unsigned int version)
3676  {
3677  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
3678  ar & BOOST_SERIALIZATION_NVP(m_pOffsetPose);
3679  }
3680  }; // Sensor
3681  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Sensor)
3685  typedef std::vector<Sensor*> SensorVector;
3686 
3690 
3694  typedef std::map<Name, Sensor*> SensorManagerMap;
3695 
3700  {
3701  public:
3706  {
3707  }
3708 
3712  virtual ~SensorManager()
3713  {
3714  }
3715 
3716  public:
3720  static SensorManager* GetInstance();
3721 
3722  public:
3730  void RegisterSensor(Sensor* pSensor, kt_bool override = false)
3731  {
3732  Validate(pSensor);
3733 
3734  if ((m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) && !override)
3735  {
3736  throw Exception("Cannot register sensor: already registered: [" +
3737  pSensor->GetName().ToString() +
3738  "] (Consider setting 'override' to true)");
3739  }
3740 
3741  std::cout << "Registering sensor: [" << pSensor->GetName().ToString() << "]" << std::endl;
3742 
3743  m_Sensors[pSensor->GetName()] = pSensor;
3744  }
3745 
3750  void UnregisterSensor(Sensor* pSensor)
3751  {
3752  Validate(pSensor);
3753 
3754  if (m_Sensors.find(pSensor->GetName()) != m_Sensors.end())
3755  {
3756  std::cout << "Unregistering sensor: " << pSensor->GetName().ToString() << std::endl;
3757 
3758  m_Sensors.erase(pSensor->GetName());
3759  }
3760  else
3761  {
3762  throw Exception("Cannot unregister sensor: not registered: [" + pSensor->GetName().ToString() + "]");
3763  }
3764  }
3765 
3771  Sensor* GetSensorByName(const Name& rName)
3772  {
3773  if (m_Sensors.find(rName) != m_Sensors.end())
3774  {
3775  return m_Sensors[rName];
3776  }
3777 
3778  throw Exception("Sensor not registered: [" + rName.ToString() + "] (Did you add the sensor to the Dataset?)");
3779  }
3780 
3786  template<class T>
3787  T* GetSensorByName(const Name& rName)
3788  {
3789  Sensor* pSensor = GetSensorByName(rName);
3790 
3791  return dynamic_cast<T*>(pSensor);
3792  }
3793 
3799  {
3800  SensorVector sensors;
3801 
3802  forEach(SensorManagerMap, &m_Sensors)
3803  {
3804  sensors.push_back(iter->second);
3805  }
3806 
3807  return sensors;
3808  }
3809 
3810  protected:
3815  static void Validate(Sensor* pSensor)
3816  {
3817  if (pSensor == NULL)
3818  {
3819  throw Exception("Invalid sensor: NULL");
3820  }
3821  else if (pSensor->GetName().ToString() == "")
3822  {
3823  throw Exception("Invalid sensor: nameless");
3824  }
3825  }
3826 
3827  protected:
3832 
3833  friend class boost::serialization::access;
3834  template<class Archive>
3835  void serialize(Archive &ar, const unsigned int version)
3836  {
3837  ar & BOOST_SERIALIZATION_NVP(m_Sensors);
3838  }
3839  };
3840 
3844 
3851  class Drive : public Sensor
3852  {
3853  public:
3854  // @cond EXCLUDE
3856  // @endcond
3857 
3858  public:
3862  Drive(const std::string& rName)
3863  : Sensor(rName)
3864  {
3865  }
3869  virtual ~Drive()
3870  {
3871  }
3872 
3873  public:
3874  virtual kt_bool Validate()
3875  {
3876  return true;
3877  }
3878 
3879  virtual kt_bool Validate(SensorData* pSensorData)
3880  {
3881  if (pSensorData == NULL)
3882  {
3883  return false;
3884  }
3885 
3886  return true;
3887  }
3888 
3889  private:
3890  Drive(const Drive&);
3891  const Drive& operator=(const Drive&);
3892  friend class boost::serialization::access;
3893  template<class Archive>
3894  void serialize(Archive &ar, const unsigned int version)
3895  {
3896  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Sensor);
3897  }
3898  }; // class Drive
3899 
3900  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Drive)
3904 
3905  class LocalizedRangeScan;
3906  class CoordinateConverter;
3907 
3921  {
3922  public:
3924  {
3925  }
3926  // @cond EXCLUDE
3928  // @endcond
3929 
3930  public:
3935  {
3936  }
3937 
3938  public:
3944  {
3945  return m_pMinimumRange->GetValue();
3946  }
3947 
3952  inline void SetMinimumRange(kt_double minimumRange)
3953  {
3954  m_pMinimumRange->SetValue(minimumRange);
3955 
3956  SetRangeThreshold(GetRangeThreshold());
3957  }
3958 
3964  {
3965  return m_pMaximumRange->GetValue();
3966  }
3967 
3972  inline void SetMaximumRange(kt_double maximumRange)
3973  {
3974  m_pMaximumRange->SetValue(maximumRange);
3975 
3976  SetRangeThreshold(GetRangeThreshold());
3977  }
3978 
3984  {
3985  return m_pRangeThreshold->GetValue();
3986  }
3987 
3992  inline void SetRangeThreshold(kt_double rangeThreshold)
3993  {
3994  // make sure rangeThreshold is within laser range finder range
3995  m_pRangeThreshold->SetValue(math::Clip(rangeThreshold, GetMinimumRange(), GetMaximumRange()));
3996 
3997  if (math::DoubleEqual(GetRangeThreshold(), rangeThreshold) == false)
3998  {
3999  std::cout << "Info: clipped range threshold to be within minimum and maximum range!" << std::endl;
4000  }
4001  }
4002 
4008  {
4009  return m_pMinimumAngle->GetValue();
4010  }
4011 
4016  inline void SetMinimumAngle(kt_double minimumAngle)
4017  {
4018  m_pMinimumAngle->SetValue(minimumAngle);
4019 
4020  Update();
4021  }
4022 
4028  {
4029  return m_pMaximumAngle->GetValue();
4030  }
4031 
4036  inline void SetMaximumAngle(kt_double maximumAngle)
4037  {
4038  m_pMaximumAngle->SetValue(maximumAngle);
4039 
4040  Update();
4041  }
4042 
4048  {
4049  return m_pAngularResolution->GetValue();
4050  }
4051 
4056  inline void SetAngularResolution(kt_double angularResolution)
4057  {
4058  if (m_pType->GetValue() == LaserRangeFinder_Custom)
4059  {
4060  m_pAngularResolution->SetValue(angularResolution);
4061  }
4062  else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS100)
4063  {
4064  if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
4065  {
4066  m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
4067  }
4068  else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
4069  {
4070  m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
4071  }
4072  else
4073  {
4074  std::stringstream stream;
4075  stream << "Invalid resolution for Sick LMS100: ";
4076  stream << angularResolution;
4077  throw Exception(stream.str());
4078  }
4079  }
4080  else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS200 ||
4081  m_pType->GetValue() == LaserRangeFinder_Sick_LMS291)
4082  {
4083  if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
4084  {
4085  m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
4086  }
4087  else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
4088  {
4089  m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
4090  }
4091  else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(1.00)))
4092  {
4093  m_pAngularResolution->SetValue(math::DegreesToRadians(1.00));
4094  }
4095  else
4096  {
4097  std::stringstream stream;
4098  stream << "Invalid resolution for Sick LMS291: ";
4099  stream << angularResolution;
4100  throw Exception(stream.str());
4101  }
4102  }
4103  else
4104  {
4105  throw Exception("Can't set angular resolution, please create a LaserRangeFinder of type Custom");
4106  }
4107 
4108  Update();
4109  }
4110 
4115  {
4116  return m_pType->GetValue();
4117  }
4118 
4124  {
4125  return m_NumberOfRangeReadings;
4126  }
4127 
4128 
4133  inline kt_bool GetIs360Laser() const
4134  {
4135  return m_pIs360Laser->GetValue();
4136  }
4137 
4142  inline void SetIs360Laser(bool is_360_laser)
4143  {
4144  m_pIs360Laser->SetValue(is_360_laser);
4145 
4146  Update();
4147  }
4148 
4149 
4150  virtual kt_bool Validate()
4151  {
4152  Update();
4153 
4154  if (math::InRange(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange()) == false)
4155  {
4156  std::cout << "Please set range threshold to a value between ["
4157  << GetMinimumRange() << ";" << GetMaximumRange() << "]" << std::endl;
4158  return false;
4159  }
4160 
4161  return true;
4162  }
4163 
4164  virtual kt_bool Validate(SensorData* pSensorData);
4165 
4173  const PointVectorDouble GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan,
4174  CoordinateConverter* pCoordinateConverter,
4175  kt_bool ignoreThresholdPoints = true,
4176  kt_bool flipY = false) const;
4177 
4178  public:
4186  {
4187  LaserRangeFinder* pLrf = NULL;
4188 
4189  switch (type)
4190  {
4191  // see http://www.hizook.com/files/publications/SICK_LMS100.pdf
4192  // set range threshold to 18m
4194  {
4195  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 100"));
4196 
4197  // Sensing range is 18 meters (at 10% reflectivity, max range of 20 meters), with an error of about 20mm
4198  pLrf->m_pMinimumRange->SetValue(0.0);
4199  pLrf->m_pMaximumRange->SetValue(20.0);
4200 
4201  // 270 degree range, 50 Hz
4204 
4205  // 0.25 degree angular resolution
4207 
4208  pLrf->m_NumberOfRangeReadings = 1081;
4209  }
4210  break;
4211 
4212  // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
4213  // set range threshold to 10m
4215  {
4216  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 200"));
4217 
4218  // Sensing range is 80 meters
4219  pLrf->m_pMinimumRange->SetValue(0.0);
4220  pLrf->m_pMaximumRange->SetValue(80.0);
4221 
4222  // 180 degree range, 75 Hz
4225 
4226  // 0.5 degree angular resolution
4228 
4229  pLrf->m_NumberOfRangeReadings = 361;
4230  }
4231  break;
4232 
4233  // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
4234  // set range threshold to 30m
4236  {
4237  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 291"));
4238 
4239  // Sensing range is 80 meters
4240  pLrf->m_pMinimumRange->SetValue(0.0);
4241  pLrf->m_pMaximumRange->SetValue(80.0);
4242 
4243  // 180 degree range, 75 Hz
4246 
4247  // 0.5 degree angular resolution
4249 
4250  pLrf->m_NumberOfRangeReadings = 361;
4251  }
4252  break;
4253 
4254  // see http://www.hizook.com/files/publications/Hokuyo_UTM_LaserRangeFinder_LIDAR.pdf
4255  // set range threshold to 30m
4257  {
4258  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo UTM-30LX"));
4259 
4260  // Sensing range is 30 meters
4261  pLrf->m_pMinimumRange->SetValue(0.1);
4262  pLrf->m_pMaximumRange->SetValue(30.0);
4263 
4264  // 270 degree range, 40 Hz
4267 
4268  // 0.25 degree angular resolution
4270 
4271  pLrf->m_NumberOfRangeReadings = 1081;
4272  }
4273  break;
4274 
4275  // see http://www.hizook.com/files/publications/HokuyoURG_Datasheet.pdf
4276  // set range threshold to 3.5m
4278  {
4279  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo URG-04LX"));
4280 
4281  // Sensing range is 4 meters. It has detection problems when
4282  // scanning absorptive surfaces (such as black trimming).
4283  pLrf->m_pMinimumRange->SetValue(0.02);
4284  pLrf->m_pMaximumRange->SetValue(4.0);
4285 
4286  // 240 degree range, 10 Hz
4289 
4290  // 0.352 degree angular resolution
4292 
4293  pLrf->m_NumberOfRangeReadings = 751;
4294  }
4295  break;
4296 
4298  {
4299  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("User-Defined LaserRangeFinder"));
4300 
4301  // Sensing range is 80 meters.
4302  pLrf->m_pMinimumRange->SetValue(0.0);
4303  pLrf->m_pMaximumRange->SetValue(80.0);
4304 
4305  // 180 degree range
4308 
4309  // 1.0 degree angular resolution
4311 
4312  pLrf->m_NumberOfRangeReadings = 181;
4313  }
4314  break;
4315  }
4316 
4317  if (pLrf != NULL)
4318  {
4319  pLrf->m_pType->SetValue(type);
4320 
4321  Pose2 defaultOffset;
4322  pLrf->SetOffsetPose(defaultOffset);
4323  }
4324 
4325  return pLrf;
4326  }
4327 
4328  private:
4332  LaserRangeFinder(const Name& rName)
4333  : Sensor(rName)
4334  , m_NumberOfRangeReadings(0)
4335  {
4336  m_pMinimumRange = new Parameter<kt_double>("MinimumRange", 0.0, GetParameterManager());
4337  m_pMaximumRange = new Parameter<kt_double>("MaximumRange", 80.0, GetParameterManager());
4338 
4339  m_pMinimumAngle = new Parameter<kt_double>("MinimumAngle", -KT_PI_2, GetParameterManager());
4340  m_pMaximumAngle = new Parameter<kt_double>("MaximumAngle", KT_PI_2, GetParameterManager());
4341 
4342  m_pAngularResolution = new Parameter<kt_double>("AngularResolution",
4344  GetParameterManager());
4345 
4346  m_pRangeThreshold = new Parameter<kt_double>("RangeThreshold", 12.0, GetParameterManager());
4347 
4348  m_pIs360Laser = new Parameter<kt_bool>("Is360DegreeLaser", false, GetParameterManager());
4349 
4350  m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager());
4351  m_pType->DefineEnumValue(LaserRangeFinder_Custom, "Custom");
4352  m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS100, "Sick_LMS100");
4353  m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS200, "Sick_LMS200");
4354  m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS291, "Sick_LMS291");
4355  m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_UTM_30LX, "Hokuyo_UTM_30LX");
4356  m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_URG_04LX, "Hokuyo_URG_04LX");
4357  }
4358 
4363  void Update()
4364  {
4365  int residual = 1;
4366  if (GetIs360Laser())
4367  {
4368  // residual is 0 by 360 lidar conventions
4369  residual = 0;
4370  }
4371  m_NumberOfRangeReadings = static_cast<kt_int32u>(math::Round((GetMaximumAngle() -
4372  GetMinimumAngle())
4373  / GetAngularResolution()) + residual);
4374  }
4375 
4376  private:
4378  const LaserRangeFinder& operator=(const LaserRangeFinder&);
4379 
4380  private:
4381  // sensor m_Parameters
4384 
4386 
4389 
4391 
4393 
4395 
4397 
4398  // static std::string LaserRangeFinderTypeNames[6];
4399  friend class boost::serialization::access;
4400  template<class Archive>
4401  void serialize(Archive &ar, const unsigned int version)
4402  {
4403  if (Archive::is_loading::value)
4404  {
4405  m_pMinimumRange = new Parameter<kt_double>("MinimumRange", 0.0, GetParameterManager());
4406  m_pMaximumRange = new Parameter<kt_double>("MaximumRange", 80.0, GetParameterManager());
4407 
4408  m_pMinimumAngle = new Parameter<kt_double>("MinimumAngle", -KT_PI_2, GetParameterManager());
4409  m_pMaximumAngle = new Parameter<kt_double>("MaximumAngle", KT_PI_2, GetParameterManager());
4410 
4411  m_pAngularResolution = new Parameter<kt_double>("AngularResolution",
4413  GetParameterManager());
4414 
4415  m_pRangeThreshold = new Parameter<kt_double>("RangeThreshold", 12.0, GetParameterManager());
4416 
4417  m_pIs360Laser = new Parameter<kt_bool>("Is360DegreeLaser", false, GetParameterManager());
4418 
4419  m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager());
4420  }
4421 
4422  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Sensor);
4423  ar & BOOST_SERIALIZATION_NVP(m_pMinimumAngle);
4424  ar & BOOST_SERIALIZATION_NVP(m_pMaximumAngle);
4425  ar & BOOST_SERIALIZATION_NVP(m_pAngularResolution);
4426  ar & BOOST_SERIALIZATION_NVP(m_pMinimumRange);
4427  ar & BOOST_SERIALIZATION_NVP(m_pMaximumRange);
4428  ar & BOOST_SERIALIZATION_NVP(m_pRangeThreshold);
4429  ar & BOOST_SERIALIZATION_NVP(m_pIs360Laser);
4430  ar & BOOST_SERIALIZATION_NVP(m_pType);
4431  ar & BOOST_SERIALIZATION_NVP(m_NumberOfRangeReadings);
4432  }
4433  }; // LaserRangeFinder
4434  BOOST_SERIALIZATION_ASSUME_ABSTRACT(LaserRangeFinder)
4438 
4442  typedef enum
4443  {
4447  } GridStates;
4448 
4452 
4459  {
4460  public:
4465  : m_Scale(20.0)
4466  {
4467  }
4468 
4469  public:
4476  {
4477  return value * m_Scale;
4478  }
4479 
4486  inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double>& rWorld, kt_bool flipY = false) const
4487  {
4488  kt_double gridX = (rWorld.GetX() - m_Offset.GetX()) * m_Scale;
4489  kt_double gridY = 0.0;
4490 
4491  if (flipY == false)
4492  {
4493  gridY = (rWorld.GetY() - m_Offset.GetY()) * m_Scale;
4494  }
4495  else
4496  {
4497  gridY = (m_Size.GetHeight() / m_Scale - rWorld.GetY() + m_Offset.GetY()) * m_Scale;
4498  }
4499 
4500  return Vector2<kt_int32s>(static_cast<kt_int32s>(math::Round(gridX)), static_cast<kt_int32s>(math::Round(gridY)));
4501  }
4502 
4509  inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s>& rGrid, kt_bool flipY = false) const
4510  {
4511  kt_double worldX = m_Offset.GetX() + rGrid.GetX() / m_Scale;
4512  kt_double worldY = 0.0;
4513 
4514  if (flipY == false)
4515  {
4516  worldY = m_Offset.GetY() + rGrid.GetY() / m_Scale;
4517  }
4518  else
4519  {
4520  worldY = m_Offset.GetY() + (m_Size.GetHeight() - rGrid.GetY()) / m_Scale;
4521  }
4522 
4523  return Vector2<kt_double>(worldX, worldY);
4524  }
4525 
4530  inline kt_double GetScale() const
4531  {
4532  return m_Scale;
4533  }
4534 
4539  inline void SetScale(kt_double scale)
4540  {
4541  m_Scale = scale;
4542  }
4543 
4548  inline const Vector2<kt_double>& GetOffset() const
4549  {
4550  return m_Offset;
4551  }
4552 
4557  inline void SetOffset(const Vector2<kt_double>& rOffset)
4558  {
4559  m_Offset = rOffset;
4560  }
4561 
4566  inline void SetSize(const Size2<kt_int32s>& rSize)
4567  {
4568  m_Size = rSize;
4569  }
4570 
4575  inline const Size2<kt_int32s>& GetSize() const
4576  {
4577  return m_Size;
4578  }
4579 
4584  inline kt_double GetResolution() const
4585  {
4586  return 1.0 / m_Scale;
4587  }
4588 
4593  inline void SetResolution(kt_double resolution)
4594  {
4595  m_Scale = 1.0 / resolution;
4596  }
4597 
4603  {
4604  BoundingBox2 box;
4605 
4606  kt_double minX = GetOffset().GetX();
4607  kt_double minY = GetOffset().GetY();
4608  kt_double maxX = minX + GetSize().GetWidth() * GetResolution();
4609  kt_double maxY = minY + GetSize().GetHeight() * GetResolution();
4610 
4611  box.SetMinimum(GetOffset());
4612  box.SetMaximum(Vector2<kt_double>(maxX, maxY));
4613  return box;
4614  }
4615 
4616  private:
4619 
4621  friend class boost::serialization::access;
4622  template<class Archive>
4623  void serialize(Archive &ar, const unsigned int version)
4624  {
4625  ar & BOOST_SERIALIZATION_NVP(m_Size);
4626  ar & BOOST_SERIALIZATION_NVP(m_Scale);
4627  ar & BOOST_SERIALIZATION_NVP(m_Offset);
4628  }
4629 
4630  }; // CoordinateConverter
4631 
4635 
4639  template<typename T>
4640  class Grid
4641  {
4642  public:
4651  {
4652  }
4653  static Grid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
4654  {
4655  Grid* pGrid = new Grid(width, height);
4656 
4657  pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution);
4658 
4659  return pGrid;
4660  }
4661 
4665  virtual ~Grid()
4666  {
4667  if (m_pData)
4668  {
4669  delete [] m_pData;
4670  }
4671  if (m_pCoordinateConverter)
4672  {
4673  delete m_pCoordinateConverter;
4674  }
4675  }
4676 
4677  public:
4681  void Clear()
4682  {
4683  memset(m_pData, 0, GetDataSize() * sizeof(T));
4684  }
4685 
4691  {
4692  Grid* pGrid = CreateGrid(GetWidth(), GetHeight(), GetResolution());
4693  pGrid->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
4694 
4695  memcpy(pGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
4696 
4697  return pGrid;
4698  }
4699 
4705  virtual void Resize(kt_int32s width, kt_int32s height)
4706  {
4707  m_Width = width;
4708  m_Height = height;
4709  m_WidthStep = math::AlignValue<kt_int32s>(width, 8);
4710 
4711  if (m_pData != NULL)
4712  {
4713  delete[] m_pData;
4714  m_pData = NULL;
4715  }
4716 
4717  try
4718  {
4719  m_pData = new T[GetDataSize()];
4720 
4721  if (m_pCoordinateConverter == NULL)
4722  {
4723  m_pCoordinateConverter = new CoordinateConverter();
4724  }
4725 
4726  m_pCoordinateConverter->SetSize(Size2<kt_int32s>(width, height));
4727  }
4728  catch(...)
4729  {
4730  m_pData = NULL;
4731 
4732  m_Width = 0;
4733  m_Height = 0;
4734  m_WidthStep = 0;
4735  }
4736 
4737  Clear();
4738  }
4739 
4744  inline kt_bool IsValidGridIndex(const Vector2<kt_int32s>& rGrid) const
4745  {
4746  return (math::IsUpTo(rGrid.GetX(), m_Width) && math::IsUpTo(rGrid.GetY(), m_Height));
4747  }
4748 
4755  virtual kt_int32s GridIndex(const Vector2<kt_int32s>& rGrid, kt_bool boundaryCheck = true) const
4756  {
4757  if (boundaryCheck == true)
4758  {
4759  if (IsValidGridIndex(rGrid) == false)
4760  {
4761  std::stringstream error;
4762  error << "Index " << rGrid << " out of range. Index must be between [0; "
4763  << m_Width << ") and [0; " << m_Height << ")";
4764  throw Exception(error.str());
4765  }
4766  }
4767 
4768  kt_int32s index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep);
4769 
4770  if (boundaryCheck == true)
4771  {
4772  assert(math::IsUpTo(index, GetDataSize()));
4773  }
4774 
4775  return index;
4776  }
4777 
4784  {
4785  Vector2<kt_int32s> grid;
4786 
4787  grid.SetY(index / m_WidthStep);
4788  grid.SetX(index - grid.GetY() * m_WidthStep);
4789 
4790  return grid;
4791  }
4792 
4799  inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double>& rWorld, kt_bool flipY = false) const
4800  {
4801  return GetCoordinateConverter()->WorldToGrid(rWorld, flipY);
4802  }
4803 
4810  inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s>& rGrid, kt_bool flipY = false) const
4811  {
4812  return GetCoordinateConverter()->GridToWorld(rGrid, flipY);
4813  }
4814 
4821  {
4822  kt_int32s index = GridIndex(rGrid, true);
4823  return m_pData + index;
4824  }
4825 
4831  T* GetDataPointer(const Vector2<kt_int32s>& rGrid) const
4832  {
4833  kt_int32s index = GridIndex(rGrid, true);
4834  return m_pData + index;
4835  }
4836 
4841  inline kt_int32s GetWidth() const
4842  {
4843  return m_Width;
4844  };
4845 
4850  inline kt_int32s GetHeight() const
4851  {
4852  return m_Height;
4853  };
4854 
4859  inline const Size2<kt_int32s> GetSize() const
4860  {
4861  return Size2<kt_int32s>(m_Width, m_Height);
4862  }
4863 
4868  inline kt_int32s GetWidthStep() const
4869  {
4870  return m_WidthStep;
4871  }
4872 
4877  inline T* GetDataPointer()
4878  {
4879  return m_pData;
4880  }
4881 
4886  inline T* GetDataPointer() const
4887  {
4888  return m_pData;
4889  }
4890 
4895  inline kt_int32s GetDataSize() const
4896  {
4897  return m_WidthStep * m_Height;
4898  }
4899 
4905  inline T GetValue(const Vector2<kt_int32s>& rGrid) const
4906  {
4907  kt_int32s index = GridIndex(rGrid);
4908  return m_pData[index];
4909  }
4910 
4916  {
4917  return m_pCoordinateConverter;
4918  }
4919 
4924  inline kt_double GetResolution() const
4925  {
4926  return GetCoordinateConverter()->GetResolution();
4927  }
4928 
4934  {
4935  return GetCoordinateConverter()->GetBoundingBox();
4936  }
4937 
4947  void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor* f = NULL)
4948  {
4949  kt_bool steep = abs(y1 - y0) > abs(x1 - x0);
4950  if (steep)
4951  {
4952  std::swap(x0, y0);
4953  std::swap(x1, y1);
4954  }
4955  if (x0 > x1)
4956  {
4957  std::swap(x0, x1);
4958  std::swap(y0, y1);
4959  }
4960 
4961  kt_int32s deltaX = x1 - x0;
4962  kt_int32s deltaY = abs(y1 - y0);
4963  kt_int32s error = 0;
4964  kt_int32s ystep;
4965  kt_int32s y = y0;
4966 
4967  if (y0 < y1)
4968  {
4969  ystep = 1;
4970  }
4971  else
4972  {
4973  ystep = -1;
4974  }
4975 
4976  kt_int32s pointX;
4977  kt_int32s pointY;
4978  for (kt_int32s x = x0; x <= x1; x++)
4979  {
4980  if (steep)
4981  {
4982  pointX = y;
4983  pointY = x;
4984  }
4985  else
4986  {
4987  pointX = x;
4988  pointY = y;
4989  }
4990 
4991  error += deltaY;
4992 
4993  if (2 * error >= deltaX)
4994  {
4995  y += ystep;
4996  error -= deltaX;
4997  }
4998 
4999  Vector2<kt_int32s> gridIndex(pointX, pointY);
5000  if (IsValidGridIndex(gridIndex))
5001  {
5002  kt_int32s index = GridIndex(gridIndex, false);
5003  T* pGridPointer = GetDataPointer();
5004  pGridPointer[index]++;
5005 
5006  if (f != NULL)
5007  {
5008  (*f)(index);
5009  }
5010  }
5011  }
5012  }
5013 
5014  protected:
5020  Grid(kt_int32s width, kt_int32s height)
5021  : m_pData(NULL)
5022  , m_pCoordinateConverter(NULL)
5023  {
5024  Resize(width, height);
5025  }
5026 
5027  private:
5028  kt_int32s m_Width; // width of grid
5029  kt_int32s m_Height; // height of grid
5030  kt_int32s m_WidthStep; // 8 bit aligned width of grid
5031  T* m_pData; // grid data
5032 
5033  // coordinate converter to convert between world coordinates and grid coordinates
5035  friend class boost::serialization::access;
5036  template<class Archive>
5037  void serialize(Archive &ar, const unsigned int version)
5038  {
5039  ar & BOOST_SERIALIZATION_NVP(m_Width);
5040  ar & BOOST_SERIALIZATION_NVP(m_Height);
5041  ar & BOOST_SERIALIZATION_NVP(m_WidthStep);
5042  ar & BOOST_SERIALIZATION_NVP(m_pCoordinateConverter);
5043 
5044 
5045  if (Archive::is_loading::value)
5046  {
5047  m_pData = new T[m_WidthStep * m_Height];
5048  }
5049  ar & boost::serialization::make_array<T>(m_pData, m_WidthStep * m_Height);
5050  }
5051 
5052  }; // Grid
5053  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Grid)
5054 
5055 
5056 
5062  class CustomData : public Object
5063  {
5064  public:
5065  // @cond EXCLUDE
5067  // @endcond
5068 
5069  public:
5074  : Object()
5075  {
5076  }
5077 
5081  virtual ~CustomData()
5082  {
5083  }
5084 
5085  public:
5090  virtual const std::string Write() const = 0;
5091 
5096  virtual void Read(const std::string& rValue) = 0;
5097 
5098  private:
5099  CustomData(const CustomData&);
5100  const CustomData& operator=(const CustomData&);
5101 
5102  friend class boost::serialization::access;
5103  template<class Archive>
5104  void serialize(Archive &ar, const unsigned int version)
5105  {
5106  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
5107  }
5108  };
5109  BOOST_SERIALIZATION_ASSUME_ABSTRACT(CustomData)
5110 
5111 
5114  typedef std::vector<CustomData*> CustomDataVector;
5115 
5119 
5124  {
5125  public:
5126  // @cond EXCLUDE
5128  // @endcond
5129 
5130  public:
5131 
5136  virtual ~SensorData();
5137 
5138  public:
5143  inline kt_int32s GetStateId() const
5144  {
5145  return m_StateId;
5146  }
5147 
5152  inline void SetStateId(kt_int32s stateId)
5153  {
5154  m_StateId = stateId;
5155  }
5156 
5161  inline kt_int32s GetUniqueId() const
5162  {
5163  return m_UniqueId;
5164  }
5165 
5170  inline void SetUniqueId(kt_int32u uniqueId)
5171  {
5172  m_UniqueId = uniqueId;
5173  }
5174 
5179  inline kt_double GetTime() const
5180  {
5181  return m_Time;
5182  }
5183 
5188  inline void SetTime(kt_double time)
5189  {
5190  m_Time = time;
5191  }
5192 
5197  inline const Name& GetSensorName() const
5198  {
5199  return m_SensorName;
5200  }
5201 
5206  inline void AddCustomData(CustomData* pCustomData)
5207  {
5208  m_CustomData.push_back(pCustomData);
5209  }
5210 
5215  inline const CustomDataVector& GetCustomData() const
5216  {
5217  return m_CustomData;
5218  }
5219 
5220  protected:
5224  SensorData(const Name& rSensorName);
5225 
5226  private:
5230  SensorData(const SensorData&);
5231 
5235  const SensorData& operator=(const SensorData&);
5236 
5237  private:
5242 
5247 
5252 
5257 
5259 
5260  friend class boost::serialization::access;
5261  template<class Archive>
5262  void serialize(Archive &ar, const unsigned int version)
5263  {
5264  ar & BOOST_SERIALIZATION_NVP(m_StateId);
5265  ar & BOOST_SERIALIZATION_NVP(m_UniqueId);
5266  ar & BOOST_SERIALIZATION_NVP(m_SensorName);
5267  ar & BOOST_SERIALIZATION_NVP(m_Time);
5268  ar & BOOST_SERIALIZATION_NVP(m_CustomData);
5269  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
5270  }
5271 };
5272 
5273 
5277 
5281  typedef std::vector<kt_double> RangeReadingsVector;
5282 
5287  {
5288  public:
5289  // @cond EXCLUDE
5291  // @endcond
5292 
5293  public:
5298  LaserRangeScan(const Name& rSensorName)
5299  : SensorData(rSensorName)
5300  , m_pRangeReadings(NULL)
5301  , m_NumberOfRangeReadings(0)
5302  {
5303  }
5304 
5306  {
5307  }
5308 
5314  LaserRangeScan(const Name& rSensorName, const RangeReadingsVector& rRangeReadings)
5315  : SensorData(rSensorName)
5316  , m_pRangeReadings(NULL)
5317  , m_NumberOfRangeReadings(0)
5318  {
5319  assert(rSensorName.ToString() != "");
5320 
5321  SetRangeReadings(rRangeReadings);
5322  }
5323 
5328  {
5329  delete [] m_pRangeReadings;
5330  m_pRangeReadings = nullptr;
5331  }
5332 
5333  public:
5338  inline kt_double* GetRangeReadings() const
5339  {
5340  return m_pRangeReadings;
5341  }
5342 
5344  {
5345  return RangeReadingsVector(m_pRangeReadings, m_pRangeReadings + m_NumberOfRangeReadings);
5346  }
5347 
5352  inline void SetRangeReadings(const RangeReadingsVector& rRangeReadings)
5353  {
5354  // ignore for now! XXXMAE BUGUBUG 05/21/2010 << TODO(lucbettaieb): What the heck is this??
5355  // if (rRangeReadings.size() != GetNumberOfRangeReadings())
5356  // {
5357  // std::stringstream error;
5358  // error << "Given number of readings (" << rRangeReadings.size()
5359  // << ") does not match expected number of range finder (" << GetNumberOfRangeReadings() << ")";
5360  // throw Exception(error.str());
5361  // }
5362 
5363  if (!rRangeReadings.empty())
5364  {
5365  if (rRangeReadings.size() != m_NumberOfRangeReadings)
5366  {
5367  // delete old readings
5368  delete [] m_pRangeReadings;
5369 
5370  // store size of array!
5371  m_NumberOfRangeReadings = static_cast<kt_int32u>(rRangeReadings.size());
5372 
5373  // allocate range readings
5374  m_pRangeReadings = new kt_double[m_NumberOfRangeReadings];
5375  }
5376 
5377  // copy readings
5378  kt_int32u index = 0;
5379  const_forEach(RangeReadingsVector, &rRangeReadings)
5380  {
5381  m_pRangeReadings[index++] = *iter;
5382  }
5383  }
5384  else
5385  {
5386  delete [] m_pRangeReadings;
5387  m_pRangeReadings = NULL;
5388  }
5389  }
5390 
5396  {
5397  return SensorManager::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorName());
5398  }
5399 
5405  {
5406  return m_NumberOfRangeReadings;
5407  }
5408 
5409  private:
5411  const LaserRangeScan& operator=(const LaserRangeScan&);
5412 
5413  private:
5416 
5417  friend class boost::serialization::access;
5418  template<class Archive>
5419  void serialize(Archive &ar, const unsigned int version)
5420  {
5421  ar & BOOST_SERIALIZATION_NVP(m_NumberOfRangeReadings);
5422  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(SensorData);
5423 
5424  if (Archive::is_loading::value)
5425  {
5426  m_pRangeReadings = new kt_double[m_NumberOfRangeReadings];
5427  }
5428  ar & boost::serialization::make_array<kt_double>(m_pRangeReadings, m_NumberOfRangeReadings);
5429  }
5430  }; // LaserRangeScan
5431 
5435 
5439  class DrivePose : public SensorData
5440  {
5441  public:
5442  // @cond EXCLUDE
5444  // @endcond
5445 
5446  public:
5451  DrivePose(const Name& rSensorName)
5452  : SensorData(rSensorName)
5453  {
5454  }
5455 
5459  virtual ~DrivePose()
5460  {
5461  }
5462 
5463  public:
5468  inline const Pose3& GetOdometricPose() const
5469  {
5470  return m_OdometricPose;
5471  }
5472 
5477  inline void SetOdometricPose(const Pose3& rPose)
5478  {
5479  m_OdometricPose = rPose;
5480  }
5481 
5482  private:
5483  DrivePose(const DrivePose&);
5484  const DrivePose& operator=(const DrivePose&);
5485 
5486  private:
5491  }; // class DrivePose
5492 
5496 
5504  {
5505  public:
5506  // @cond EXCLUDE
5508  // @endcond
5509 
5510  public:
5514  LocalizedRangeScan(const Name& rSensorName, const RangeReadingsVector& rReadings)
5515  : LaserRangeScan(rSensorName, rReadings)
5516  , m_IsDirty(true)
5517  {
5518  }
5519 
5521  {}
5522 
5527  {
5528  }
5529 
5530  private:
5531  mutable boost::shared_mutex m_Lock;
5532 
5533  public:
5538  inline const Pose2& GetOdometricPose() const
5539  {
5540  return m_OdometricPose;
5541  }
5542 
5547  inline void SetOdometricPose(const Pose2& rPose)
5548  {
5549  m_OdometricPose = rPose;
5550  }
5551 
5560  inline const Pose2& GetCorrectedPose() const
5561  {
5562  return m_CorrectedPose;
5563  }
5564 
5569  inline void SetCorrectedPose(const Pose2& rPose)
5570  {
5571  m_CorrectedPose = rPose;
5572 
5573  m_IsDirty = true;
5574  }
5575 
5579  inline const Pose2& GetBarycenterPose() const
5580  {
5581  boost::shared_lock<boost::shared_mutex> lock(m_Lock);
5582  if (m_IsDirty)
5583  {
5584  // throw away constness and do an update!
5585  lock.unlock();
5586  boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
5587  const_cast<LocalizedRangeScan*>(this)->Update();
5588  }
5589 
5590  return m_BarycenterPose;
5591  }
5592 
5593  inline void SetBarycenterPose(Pose2& bcenter)
5594  {
5595  m_BarycenterPose = bcenter;
5596  }
5597 
5603  inline Pose2 GetReferencePose(kt_bool useBarycenter) const
5604  {
5605  boost::shared_lock<boost::shared_mutex> lock(m_Lock);
5606  if (m_IsDirty)
5607  {
5608  // throw away constness and do an update!
5609  lock.unlock();
5610  boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
5611  const_cast<LocalizedRangeScan*>(this)->Update();
5612  }
5613 
5614  return useBarycenter ? GetBarycenterPose() : GetSensorPose();
5615  }
5616 
5621  inline Pose2 GetSensorPose() const
5622  {
5623  return GetSensorAt(m_CorrectedPose);
5624  }
5625 
5626  inline void SetIsDirty(kt_bool& rIsDirty)
5627  {
5628  m_IsDirty = rIsDirty;
5629  }
5630 
5635  void SetSensorPose(const Pose2& rScanPose)
5636  {
5637  Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose();
5638  kt_double offsetLength = deviceOffsetPose2.GetPosition().Length();
5639  kt_double offsetHeading = deviceOffsetPose2.GetHeading();
5640  kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX());
5641  kt_double correctedHeading = math::NormalizeAngle(rScanPose.GetHeading());
5642  Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading),
5643  offsetLength * sin(correctedHeading + angleoffset - offsetHeading),
5644  offsetHeading);
5645 
5646  m_CorrectedPose = rScanPose - worldSensorOffset;
5647 
5648  Update();
5649  }
5650 
5656  inline Pose2 GetSensorAt(const Pose2& rPose) const
5657  {
5658  return Transform(rPose).TransformPose(GetLaserRangeFinder()->GetOffsetPose());
5659  }
5660 
5665  inline const BoundingBox2& GetBoundingBox() const
5666  {
5667  boost::shared_lock<boost::shared_mutex> lock(m_Lock);
5668  if (m_IsDirty)
5669  {
5670  // throw away constness and do an update!
5671  lock.unlock();
5672  boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
5673  const_cast<LocalizedRangeScan*>(this)->Update();
5674  }
5675 
5676  return m_BoundingBox;
5677  }
5678 
5679  inline void SetBoundingBox(BoundingBox2& bbox)
5680  {
5681  m_BoundingBox = bbox;
5682  }
5683 
5687  inline const PointVectorDouble& GetPointReadings(kt_bool wantFiltered = false) const
5688  {
5689  boost::shared_lock<boost::shared_mutex> lock(m_Lock);
5690  if (m_IsDirty)
5691  {
5692  // throw away constness and do an update!
5693  lock.unlock();
5694  boost::unique_lock<boost::shared_mutex> uniqueLock(m_Lock);
5695  const_cast<LocalizedRangeScan*>(this)->Update();
5696  }
5697 
5698  if (wantFiltered == true)
5699  {
5700  return m_PointReadings;
5701  }
5702  else
5703  {
5704  return m_UnfilteredPointReadings;
5705  }
5706  }
5707 
5708  inline void SetPointReadings(PointVectorDouble& points, kt_bool setFiltered = false)
5709  {
5710  if (setFiltered)
5711  {
5712  m_PointReadings = points;
5713  }
5714  else
5715  {
5716  m_UnfilteredPointReadings = points;
5717  }
5718  }
5719 
5720  private:
5725  virtual void Update()
5726  {
5727  LaserRangeFinder* pLaserRangeFinder = GetLaserRangeFinder();
5728 
5729  if (pLaserRangeFinder != NULL)
5730  {
5731  m_PointReadings.clear();
5732  m_UnfilteredPointReadings.clear();
5733 
5734  kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold();
5735  kt_double minimumAngle = pLaserRangeFinder->GetMinimumAngle();
5736  kt_double angularResolution = pLaserRangeFinder->GetAngularResolution();
5737  Pose2 scanPose = GetSensorPose();
5738 
5739  // compute point readings
5740  Vector2<kt_double> rangePointsSum;
5741  kt_int32u beamNum = 0;
5742  for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++)
5743  {
5744  kt_double rangeReading = GetRangeReadings()[i];
5745  if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold))
5746  {
5747  kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
5748 
5749  Vector2<kt_double> point;
5750  point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
5751  point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
5752 
5753  m_UnfilteredPointReadings.push_back(point);
5754  continue;
5755  }
5756 
5757  kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
5758 
5759  Vector2<kt_double> point;
5760  point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
5761  point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
5762 
5763  m_PointReadings.push_back(point);
5764  m_UnfilteredPointReadings.push_back(point);
5765 
5766  rangePointsSum += point;
5767  }
5768 
5769  // compute barycenter
5770  kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
5771  if (nPoints != 0.0)
5772  {
5773  Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
5774  m_BarycenterPose = Pose2(averagePosition, 0.0);
5775  }
5776  else
5777  {
5778  m_BarycenterPose = scanPose;
5779  }
5780 
5781  // calculate bounding box of scan
5782  m_BoundingBox = BoundingBox2();
5783  m_BoundingBox.Add(scanPose.GetPosition());
5784  forEach(PointVectorDouble, &m_PointReadings)
5785  {
5786  m_BoundingBox.Add(*iter);
5787  }
5788  }
5789 
5790  m_IsDirty = false;
5791  }
5792 
5796  friend class boost::serialization::access;
5797  template<class Archive>
5798  void serialize(Archive &ar, const unsigned int version)
5799  {
5800  ar & BOOST_SERIALIZATION_NVP(m_OdometricPose);
5801  ar & BOOST_SERIALIZATION_NVP(m_CorrectedPose);
5802  ar & BOOST_SERIALIZATION_NVP(m_BarycenterPose);
5803  ar & BOOST_SERIALIZATION_NVP(m_PointReadings);
5804  ar & BOOST_SERIALIZATION_NVP(m_UnfilteredPointReadings);
5805  ar & BOOST_SERIALIZATION_NVP(m_BoundingBox);
5806  ar & BOOST_SERIALIZATION_NVP(m_IsDirty);
5807  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(LaserRangeScan);
5808  }
5809 
5810 
5811  private:
5813  const LocalizedRangeScan& operator=(const LocalizedRangeScan&);
5814 
5815  private:
5820 
5825 
5826  protected:
5831 
5836 
5841 
5846 
5851  }; // LocalizedRangeScan
5852 
5856  typedef std::vector<LocalizedRangeScan*> LocalizedRangeScanVector;
5857  typedef std::map<int, LocalizedRangeScan*> LocalizedRangeScanMap;
5861 
5866  {
5867  public:
5868  // @cond EXCLUDE
5870  // @endcond
5871 
5872  public:
5877  LocalizedRangeScanWithPoints(const Name& rSensorName, const RangeReadingsVector& rReadings,
5878  const PointVectorDouble& rPoints)
5879  : m_Points(rPoints),
5880  LocalizedRangeScan(rSensorName, rReadings)
5881  {
5882  }
5883 
5888  {
5889  }
5890 
5891  private:
5895  void Update()
5896  {
5897  m_PointReadings.clear();
5898  m_UnfilteredPointReadings.clear();
5899 
5900  Pose2 scanPose = GetSensorPose();
5901  Pose2 robotPose = GetCorrectedPose();
5902 
5903  // update point readings
5904  Vector2<kt_double> rangePointsSum;
5905  for (kt_int32u i = 0; i < m_Points.size(); i++)
5906  {
5907  // check if this has a NaN
5908  if (!std::isfinite(m_Points[i].GetX()) || !std::isfinite(m_Points[i].GetY()))
5909  {
5910  Vector2<kt_double> point(m_Points[i].GetX(), m_Points[i].GetY());
5911  m_UnfilteredPointReadings.push_back(point);
5912 
5913  continue;
5914  }
5915 
5916  // transform into world coords
5917  Pose2 pointPose(m_Points[i].GetX(), m_Points[i].GetY(), 0);
5918  Pose2 result = Transform(robotPose).TransformPose(pointPose);
5919  Vector2<kt_double> point(result.GetX(), result.GetY());
5920 
5921  m_PointReadings.push_back(point);
5922  m_UnfilteredPointReadings.push_back(point);
5923 
5924  rangePointsSum += point;
5925  }
5926 
5927  // compute barycenter
5928  kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
5929  if (nPoints != 0.0)
5930  {
5931  Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
5932  m_BarycenterPose = Pose2(averagePosition, 0.0);
5933  }
5934  else
5935  {
5936  m_BarycenterPose = scanPose;
5937  }
5938 
5939  // calculate bounding box of scan
5940  m_BoundingBox = BoundingBox2();
5941  m_BoundingBox.Add(scanPose.GetPosition());
5942  forEach(PointVectorDouble, &m_PointReadings)
5943  {
5944  m_BoundingBox.Add(*iter);
5945  }
5946 
5947  m_IsDirty = false;
5948  }
5949 
5950  private:
5953 
5954  private:
5956  }; // LocalizedRangeScanWithPoints
5957 
5961 
5962  class OccupancyGrid;
5963 
5965  {
5966  public:
5968  : m_pOccupancyGrid(pGrid)
5969  {
5970  }
5971 
5976  virtual void operator() (kt_int32u index);
5977 
5978  private:
5980  }; // CellUpdater
5981 
5985  class OccupancyGrid : public Grid<kt_int8u>
5986  {
5987  friend class CellUpdater;
5988  friend class IncrementalOccupancyGrid;
5989 
5990  public:
5998  OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2<kt_double>& rOffset, kt_double resolution)
5999  : Grid<kt_int8u>(width, height)
6000  , m_pCellPassCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
6001  , m_pCellHitsCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
6002  , m_pCellUpdater(NULL)
6003  {
6004  m_pCellUpdater = new CellUpdater(this);
6005 
6006  if (karto::math::DoubleEqual(resolution, 0.0))
6007  {
6008  throw Exception("Resolution cannot be 0");
6009  }
6010 
6011  m_pMinPassThrough = new Parameter<kt_int32u>("MinPassThrough", 2);
6012  m_pOccupancyThreshold = new Parameter<kt_double>("OccupancyThreshold", 0.1);
6013 
6014  GetCoordinateConverter()->SetScale(1.0 / resolution);
6015  GetCoordinateConverter()->SetOffset(rOffset);
6016  }
6017 
6021  virtual ~OccupancyGrid()
6022  {
6023  delete m_pCellUpdater;
6024 
6025  delete m_pCellPassCnt;
6026  delete m_pCellHitsCnt;
6027 
6028  delete m_pMinPassThrough;
6029  delete m_pOccupancyThreshold;
6030  }
6031 
6032  public:
6039  {
6040  if (rScans.empty())
6041  {
6042  return NULL;
6043  }
6044 
6045  kt_int32s width, height;
6046  Vector2<kt_double> offset;
6047  ComputeDimensions(rScans, resolution, width, height, offset);
6048  OccupancyGrid* pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);
6049  pOccupancyGrid->CreateFromScans(rScans);
6050 
6051  return pOccupancyGrid;
6052  }
6053 
6059  {
6060  OccupancyGrid* pOccupancyGrid = new OccupancyGrid(GetWidth(),
6061  GetHeight(),
6062  GetCoordinateConverter()->GetOffset(),
6063  1.0 / GetCoordinateConverter()->GetScale());
6064  memcpy(pOccupancyGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
6065 
6066  pOccupancyGrid->GetCoordinateConverter()->SetSize(GetCoordinateConverter()->GetSize());
6067  pOccupancyGrid->m_pCellPassCnt = m_pCellPassCnt->Clone();
6068  pOccupancyGrid->m_pCellHitsCnt = m_pCellHitsCnt->Clone();
6069 
6070  return pOccupancyGrid;
6071  }
6072 
6078  virtual kt_bool IsFree(const Vector2<kt_int32s>& rPose) const
6079  {
6080  kt_int8u* pOffsets = reinterpret_cast<kt_int8u*>(GetDataPointer(rPose));
6081  if (*pOffsets == GridStates_Free)
6082  {
6083  return true;
6084  }
6085 
6086  return false;
6087  }
6088 
6096  virtual kt_double RayCast(const Pose2& rPose2, kt_double maxRange) const
6097  {
6098  double scale = GetCoordinateConverter()->GetScale();
6099 
6100  kt_double x = rPose2.GetX();
6101  kt_double y = rPose2.GetY();
6102  kt_double theta = rPose2.GetHeading();
6103 
6104  kt_double sinTheta = sin(theta);
6105  kt_double cosTheta = cos(theta);
6106 
6107  kt_double xStop = x + maxRange * cosTheta;
6108  kt_double xSteps = 1 + fabs(xStop - x) * scale;
6109 
6110  kt_double yStop = y + maxRange * sinTheta;
6111  kt_double ySteps = 1 + fabs(yStop - y) * scale;
6112 
6113  kt_double steps = math::Maximum(xSteps, ySteps);
6114  kt_double delta = maxRange / steps;
6115  kt_double distance = delta;
6116 
6117  for (kt_int32u i = 1; i < steps; i++)
6118  {
6119  kt_double x1 = x + distance * cosTheta;
6120  kt_double y1 = y + distance * sinTheta;
6121 
6122  Vector2<kt_int32s> gridIndex = WorldToGrid(Vector2<kt_double>(x1, y1));
6123  if (IsValidGridIndex(gridIndex) && IsFree(gridIndex))
6124  {
6125  distance = (i + 1) * delta;
6126  }
6127  else
6128  {
6129  break;
6130  }
6131  }
6132 
6133  return (distance < maxRange)? distance : maxRange;
6134  }
6135 
6142  {
6143  m_pMinPassThrough->SetValue(count);
6144  }
6145 
6151  {
6152  m_pOccupancyThreshold->SetValue(thresh);
6153  }
6154 
6155  protected:
6161  {
6162  return m_pCellHitsCnt;
6163  }
6164 
6170  {
6171  return m_pCellPassCnt;
6172  }
6173 
6174  protected:
6183  static void ComputeDimensions(const LocalizedRangeScanVector& rScans,
6184  kt_double resolution,
6185  kt_int32s& rWidth,
6186  kt_int32s& rHeight,
6187  Vector2<kt_double>& rOffset)
6188  {
6189  BoundingBox2 boundingBox;
6190 
6192  {
6193  if (*iter == nullptr)
6194  {
6195  continue;
6196  }
6197 
6198  boundingBox.Add((*iter)->GetBoundingBox());
6199  }
6200 
6201  kt_double scale = 1.0 / resolution;
6202  Size2<kt_double> size = boundingBox.GetSize();
6203 
6204  rWidth = static_cast<kt_int32s>(math::Round(size.GetWidth() * scale));
6205  rHeight = static_cast<kt_int32s>(math::Round(size.GetHeight() * scale));
6206  rOffset = boundingBox.GetMinimum();
6207  }
6208 
6213  virtual void CreateFromScans(const LocalizedRangeScanVector& rScans)
6214  {
6215  m_pCellPassCnt->Resize(GetWidth(), GetHeight());
6216  m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
6217 
6218  m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
6219  m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
6220 
6222  {
6223  if (*iter == nullptr)
6224  {
6225  continue;
6226  }
6227 
6228  LocalizedRangeScan* pScan = *iter;
6229  AddScan(pScan);
6230  }
6231 
6232  Update();
6233  }
6234 
6242  virtual kt_bool AddScan(LocalizedRangeScan* pScan, kt_bool doUpdate = false)
6243  {
6244  LaserRangeFinder* laserRangeFinder = pScan->GetLaserRangeFinder();
6245  kt_double rangeThreshold = laserRangeFinder->GetRangeThreshold();
6246  kt_double maxRange = laserRangeFinder->GetMaximumRange();
6247  kt_double minRange = laserRangeFinder->GetMinimumRange();
6248 
6249  Vector2<kt_double> scanPosition = pScan->GetSensorPose().GetPosition();
6250  // get scan point readings
6251  const PointVectorDouble& rPointReadings = pScan->GetPointReadings(false);
6252 
6253  kt_bool isAllInMap = true;
6254 
6255  // draw lines from scan position to all point readings
6256  int pointIndex = 0;
6257  const_forEachAs(PointVectorDouble, &rPointReadings, pointsIter)
6258  {
6259  Vector2<kt_double> point = *pointsIter;
6260  kt_double rangeReading = pScan->GetRangeReadings()[pointIndex];
6261  kt_bool isEndPointValid = rangeReading < (rangeThreshold - KT_TOLERANCE);
6262 
6263  if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading))
6264  {
6265  // ignore these readings
6266  pointIndex++;
6267  continue;
6268  }
6269  else if (rangeReading >= rangeThreshold)
6270  {
6271  // trace up to range reading
6272  kt_double ratio = rangeThreshold / rangeReading;
6273  kt_double dx = point.GetX() - scanPosition.GetX();
6274  kt_double dy = point.GetY() - scanPosition.GetY();
6275  point.SetX(scanPosition.GetX() + ratio * dx);
6276  point.SetY(scanPosition.GetY() + ratio * dy);
6277  }
6278 
6279  kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate);
6280  if (!isInMap)
6281  {
6282  isAllInMap = false;
6283  }
6284 
6285  pointIndex++;
6286  }
6287 
6288  return isAllInMap;
6289  }
6290 
6300  virtual kt_bool RayTrace(const Vector2<kt_double>& rWorldFrom,
6301  const Vector2<kt_double>& rWorldTo,
6302  kt_bool isEndPointValid,
6303  kt_bool doUpdate = false)
6304  {
6305  assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
6306 
6307  Vector2<kt_int32s> gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom);
6308  Vector2<kt_int32s> gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo);
6309 
6310  CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL;
6311  m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater);
6312 
6313  // for the end point
6314  if (isEndPointValid)
6315  {
6316  if (m_pCellPassCnt->IsValidGridIndex(gridTo))
6317  {
6318  kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false);
6319 
6320  kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
6321  kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
6322 
6323  // increment cell pass through and hit count
6324  pCellPassCntPtr[index]++;
6325  pCellHitCntPtr[index]++;
6326 
6327  if (doUpdate)
6328  {
6329  (*m_pCellUpdater)(index);
6330  }
6331  }
6332  }
6333 
6334  return m_pCellPassCnt->IsValidGridIndex(gridTo);
6335  }
6336 
6343  virtual void UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
6344  {
6345  if (cellPassCnt > m_pMinPassThrough->GetValue())
6346  {
6347  kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);
6348 
6349  if (hitRatio > m_pOccupancyThreshold->GetValue())
6350  {
6351  *pCell = GridStates_Occupied;
6352  }
6353  else
6354  {
6355  *pCell = GridStates_Free;
6356  }
6357  }
6358  }
6359 
6363  virtual void Update()
6364  {
6365  assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
6366 
6367  // clear grid
6368  Clear();
6369 
6370  // set occupancy status of cells
6371  kt_int8u* pDataPtr = GetDataPointer();
6372  kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
6373  kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
6374 
6375  kt_int32u nBytes = GetDataSize();
6376  for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++)
6377  {
6378  UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);
6379  }
6380  }
6381 
6387  virtual void Resize(kt_int32s width, kt_int32s height)
6388  {
6389  Grid<kt_int8u>::Resize(width, height);
6390  m_pCellPassCnt->Resize(width, height);
6391  m_pCellHitsCnt->Resize(width, height);
6392  }
6393 
6394  protected:
6399 
6404 
6405  private:
6409  OccupancyGrid(const OccupancyGrid&);
6410 
6414  const OccupancyGrid& operator=(const OccupancyGrid&);
6415 
6416  private:
6418 
6420  // NOTE: These two values are dependent on the resolution. If the resolution is too small,
6421  // then not many beams will hit the cell!
6422 
6423  // Number of beams that must pass through a cell before it will be considered to be occupied
6424  // or unoccupied. This prevents stray beams from messing up the map.
6426 
6427  // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied
6429  }; // OccupancyGrid
6430 
6434 
6439  class DatasetInfo : public Object
6440  {
6441  public:
6442  // @cond EXCLUDE
6444  // @endcond
6445 
6446  public:
6448  : Object()
6449  {
6450  m_pTitle = new Parameter<std::string>("Title", "", GetParameterManager());
6451  m_pAuthor = new Parameter<std::string>("Author", "", GetParameterManager());
6452  m_pDescription = new Parameter<std::string>("Description", "", GetParameterManager());
6453  m_pCopyright = new Parameter<std::string>("Copyright", "", GetParameterManager());
6454  }
6455 
6456  virtual ~DatasetInfo()
6457  {
6458  }
6459 
6460  public:
6464  const std::string& GetTitle() const
6465  {
6466  return m_pTitle->GetValue();
6467  }
6468 
6472  const std::string& GetAuthor() const
6473  {
6474  return m_pAuthor->GetValue();
6475  }
6476 
6480  const std::string& GetDescription() const
6481  {
6482  return m_pDescription->GetValue();
6483  }
6484 
6488  const std::string& GetCopyright() const
6489  {
6490  return m_pCopyright->GetValue();
6491  }
6492 
6497  private:
6498  DatasetInfo(const DatasetInfo&);
6499  const DatasetInfo& operator=(const DatasetInfo&);
6500 
6501  private:
6506  friend class boost::serialization::access;
6507  template<class Archive>
6508  void serialize(Archive &ar, const unsigned int version)
6509  {
6510  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
6511  ar & BOOST_SERIALIZATION_NVP(*m_pTitle);
6512  ar & BOOST_SERIALIZATION_NVP(*m_pAuthor);
6513  ar & BOOST_SERIALIZATION_NVP(*m_pDescription);
6514  ar & BOOST_SERIALIZATION_NVP(*m_pCopyright);
6515  }
6516  }; // class DatasetInfo
6517 
6521 
6526  class Dataset
6527  {
6528  public:
6533  : m_pDatasetInfo(NULL)
6534  {
6535  }
6536 
6540  virtual ~Dataset()
6541  {
6542  Clear();
6543  }
6544 
6549  void SaveToFile(const std::string& filename)
6550  {
6551  printf("Save To File\n");
6552  std::ofstream ofs(filename.c_str());
6553  boost::archive::binary_oarchive oa(ofs, boost::archive::no_codecvt);
6554  oa << BOOST_SERIALIZATION_NVP(*this);
6555  }
6556 
6561  void LoadFromFile(const std::string& filename)
6562  {
6563  printf("Load From File\n");
6564  std::ifstream ifs(filename.c_str());
6565  boost::archive::binary_iarchive ia(ifs, boost::archive::no_codecvt); //no second arg?
6566  ia >> BOOST_SERIALIZATION_NVP(*this);
6567  }
6568 
6569  public:
6574  void Add(Object* pObject, kt_bool overrideSensorName = false)
6575  {
6576  if (pObject != NULL)
6577  {
6578  if (dynamic_cast<Sensor*>(pObject))
6579  {
6580  Sensor* pSensor = dynamic_cast<Sensor*>(pObject);
6581  if (pSensor != NULL)
6582  {
6583  m_SensorNameLookup[pSensor->GetName()] = pSensor;
6584  karto::SensorManager::GetInstance()->RegisterSensor(pSensor, overrideSensorName);
6585  }
6586 
6587  m_Lasers.push_back(pObject);
6588  }
6589  else if (dynamic_cast<SensorData*>(pObject))
6590  {
6591  SensorData* pSensorData = dynamic_cast<SensorData*>(pObject);
6592  m_Data.insert({pSensorData->GetUniqueId(), pSensorData});
6593  }
6594  else if (dynamic_cast<DatasetInfo*>(pObject))
6595  {
6596  m_pDatasetInfo = dynamic_cast<DatasetInfo*>(pObject);
6597  }
6598  else
6599  {
6600  std::cout << "Did not save object of non-data and non-sensor type" << std::endl;
6601  }
6602  }
6603  }
6604 
6609  inline const ObjectVector& GetLasers() const
6610  {
6611  return m_Lasers;
6612  }
6613 
6618  inline const DataMap& GetData() const
6619  {
6620  return m_Data;
6621  }
6622 
6627  inline void RemoveData(LocalizedRangeScan* scan)
6628  {
6629  auto iterator = m_Data.find(scan->GetUniqueId());
6630  if (iterator != m_Data.end())
6631  {
6632  delete iterator->second;
6633  iterator->second = nullptr;
6634  m_Data.erase(iterator);
6635  }
6636  else
6637  {
6638  std::cout << "Failed to remove data. Pointer to LocalizedRangeScan could not be found in dataset. "
6639  << "Most likely different pointer address but same object TODO STEVE." << std::endl;
6640  }
6641  }
6642 
6648  {
6649  return m_pDatasetInfo;
6650  }
6651 
6655  virtual void Clear()
6656  {
6657  for (std::map<Name, Sensor*>::iterator iter = m_SensorNameLookup.begin(); iter != m_SensorNameLookup.end(); ++iter)
6658  {
6660  }
6661 
6662  forEach(ObjectVector, &m_Lasers)
6663  {
6664  if(*iter)
6665  {
6666  delete *iter;
6667  *iter = NULL;
6668  }
6669  }
6670 
6671  for (auto iter = m_Data.begin(); iter != m_Data.end(); ++iter)
6672  {
6673  if(iter->second)
6674  {
6675  delete iter->second;
6676  iter->second = NULL;
6677  }
6678  }
6679 
6680  m_Lasers.clear();
6681  m_Data.clear();
6682 
6683  if (m_pDatasetInfo != NULL)
6684  {
6685  delete m_pDatasetInfo;
6686  m_pDatasetInfo = NULL;
6687  }
6688  }
6689 
6690  private:
6691  std::map<Name, Sensor*> m_SensorNameLookup;
6698  friend class boost::serialization::access;
6699  template<class Archive>
6700  void serialize(Archive &ar, const unsigned int version)
6701  {
6702  std::cout<<"**Serializing Dataset**\n";
6703  std::cout<<"Dataset <- m_SensorNameLookup\n";
6704  ar & BOOST_SERIALIZATION_NVP(m_SensorNameLookup);
6705  std::cout<<"Dataset <- m_Data\n";
6706  ar & BOOST_SERIALIZATION_NVP(m_Data);
6707  std::cout<<"Dataset <- m_Lasers\n";
6708  ar & BOOST_SERIALIZATION_NVP(m_Lasers);
6709  std::cout<<"Dataset <- m_pDatasetInfo\n";
6710  ar & BOOST_SERIALIZATION_NVP(m_pDatasetInfo);
6711  std::cout<<"**Finished serializing Dataset**\n";
6712  }
6713 
6714  }; // Dataset
6715  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Dataset)
6719 
6725  {
6726  public:
6731  : m_pArray(NULL)
6732  , m_Capacity(0)
6733  , m_Size(0)
6734  {
6735  }
6736 
6740  virtual ~LookupArray()
6741  {
6742  assert(m_pArray != NULL);
6743 
6744  delete[] m_pArray;
6745  m_pArray = NULL;
6746  }
6747 
6748  public:
6752  void Clear()
6753  {
6754  memset(m_pArray, 0, sizeof(kt_int32s) * m_Capacity);
6755  }
6756 
6762  {
6763  return m_Size;
6764  }
6765 
6770  void SetSize(kt_int32u size)
6771  {
6772  assert(size != 0);
6773 
6774  if (size > m_Capacity)
6775  {
6776  if (m_pArray != NULL)
6777  {
6778  delete [] m_pArray;
6779  }
6780  m_Capacity = size;
6781  m_pArray = new kt_int32s[m_Capacity];
6782  }
6783 
6784  m_Size = size;
6785  }
6786 
6792  inline kt_int32s& operator [] (kt_int32u index)
6793  {
6794  assert(index < m_Size);
6795 
6796  return m_pArray[index];
6797  }
6798 
6804  inline kt_int32s operator [] (kt_int32u index) const
6805  {
6806  assert(index < m_Size);
6807 
6808  return m_pArray[index];
6809  }
6810 
6816  {
6817  return m_pArray;
6818  }
6819 
6824  inline kt_int32s* GetArrayPointer() const
6825  {
6826  return m_pArray;
6827  }
6828 
6829  private:
6833  friend class boost::serialization::access;
6834  template<class Archive>
6835  void serialize(Archive &ar, const unsigned int version)
6836  {
6837  ar & BOOST_SERIALIZATION_NVP(m_Capacity);
6838  ar & BOOST_SERIALIZATION_NVP(m_Size);
6839  if (Archive::is_loading::value)
6840  {
6841  m_pArray = new kt_int32s[m_Capacity];
6842  }
6843  ar & boost::serialization::make_array<kt_int32s >(m_pArray, m_Capacity);
6844 
6845 
6846  }
6847  }; // LookupArray
6848 
6852 
6866  template<typename T>
6868  {
6869  public:
6875  {
6876  }
6878  : m_pGrid(pGrid)
6879  , m_Capacity(0)
6880  , m_Size(0)
6881  , m_ppLookupArray(NULL)
6882  {
6883  }
6884 
6889  {
6890  DestroyArrays();
6891  }
6892 
6893  public:
6900  {
6901  assert(math::IsUpTo(index, m_Size));
6902 
6903  return m_ppLookupArray[index];
6904  }
6905 
6910  const std::vector<kt_double>& GetAngles() const
6911  {
6912  return m_Angles;
6913  }
6914 
6923  kt_double angleCenter,
6924  kt_double angleOffset,
6925  kt_double angleResolution)
6926  {
6927  assert(angleOffset != 0.0);
6928  assert(angleResolution != 0.0);
6929 
6930  kt_int32u nAngles = static_cast<kt_int32u>(math::Round(angleOffset * 2.0 / angleResolution) + 1);
6931  SetSize(nAngles);
6932 
6934  // convert points into local coordinates of scan pose
6935 
6936  const PointVectorDouble& rPointReadings = pScan->GetPointReadings();
6937 
6938  // compute transform to scan pose
6939  Transform transform(pScan->GetSensorPose());
6940 
6941  Pose2Vector localPoints;
6942  const_forEach(PointVectorDouble, &rPointReadings)
6943  {
6944  // do inverse transform to get points in local coordinates
6945  Pose2 vec = transform.InverseTransformPose(Pose2(*iter, 0.0));
6946  localPoints.push_back(vec);
6947  }
6948 
6950  // create lookup array for different angles
6951  kt_double angle = 0.0;
6952  kt_double startAngle = angleCenter - angleOffset;
6953  for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
6954  {
6955  angle = startAngle + angleIndex * angleResolution;
6956  ComputeOffsets(angleIndex, angle, localPoints, pScan);
6957  }
6958  // assert(math::DoubleEqual(angle, angleCenter + angleOffset));
6959  }
6960 
6961  private:
6968  void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2Vector& rLocalPoints, LocalizedRangeScan* pScan)
6969  {
6970  m_ppLookupArray[angleIndex]->SetSize(static_cast<kt_int32u>(rLocalPoints.size()));
6971  m_Angles.at(angleIndex) = angle;
6972 
6973  // set up point array by computing relative offsets to points readings
6974  // when rotated by given angle
6975 
6976  const Vector2<kt_double>& rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset();
6977 
6978  kt_double cosine = cos(angle);
6979  kt_double sine = sin(angle);
6980 
6981  kt_int32u readingIndex = 0;
6982 
6983  kt_int32s* pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer();
6984 
6985  kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
6986 
6987  const_forEach(Pose2Vector, &rLocalPoints)
6988  {
6989  const Vector2<kt_double>& rPosition = iter->GetPosition();
6990 
6991  if (std::isnan(pScan->GetRangeReadings()[readingIndex]) || std::isinf(pScan->GetRangeReadings()[readingIndex]))
6992  {
6993  pAngleIndexPointer[readingIndex] = INVALID_SCAN;
6994  readingIndex++;
6995  continue;
6996  }
6997 
6998 
6999  // counterclockwise rotation and that rotation is about the origin (0, 0).
7000  Vector2<kt_double> offset;
7001  offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY());
7002  offset.SetY(sine * rPosition.GetX() + cosine * rPosition.GetY());
7003 
7004  // have to compensate for the grid offset when getting the grid index
7005  Vector2<kt_int32s> gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset);
7006 
7007  // use base GridIndex to ignore ROI
7008  kt_int32s lookupIndex = m_pGrid->Grid<T>::GridIndex(gridPoint, false);
7009 
7010  pAngleIndexPointer[readingIndex] = lookupIndex;
7011 
7012  readingIndex++;
7013  }
7014  assert(readingIndex == rLocalPoints.size());
7015  }
7016 
7021  void SetSize(kt_int32u size)
7022  {
7023  assert(size != 0);
7024 
7025  if (size > m_Capacity)
7026  {
7027  if (m_ppLookupArray != NULL)
7028  {
7029  DestroyArrays();
7030  }
7031 
7032  m_Capacity = size;
7033  m_ppLookupArray = new LookupArray*[m_Capacity];
7034  for (kt_int32u i = 0; i < m_Capacity; i++)
7035  {
7036  m_ppLookupArray[i] = new LookupArray();
7037  }
7038  }
7039 
7040  m_Size = size;
7041 
7042  m_Angles.resize(size);
7043  }
7044 
7049  {
7050  if (m_ppLookupArray)
7051  {
7052  for (kt_int32u i = 0; i < m_Capacity; i++)
7053  {
7054  delete m_ppLookupArray[i];
7055  }
7056  }
7057  if (m_ppLookupArray)
7058  {
7059  delete[] m_ppLookupArray;
7060  m_ppLookupArray = NULL;
7061  }
7062  }
7063 
7064  private:
7066 
7069 
7071 
7072  // for sanity check
7073  std::vector<kt_double> m_Angles;
7074  friend class boost::serialization::access;
7075  template<class Archive>
7076  void serialize(Archive &ar, const unsigned int version)
7077  {
7078  ar & BOOST_SERIALIZATION_NVP(m_pGrid);
7079  ar & BOOST_SERIALIZATION_NVP(m_Capacity);
7080  ar & BOOST_SERIALIZATION_NVP(m_Size);
7081  ar & BOOST_SERIALIZATION_NVP(m_Angles);
7082  if (Archive::is_loading::value)
7083  {
7084  m_ppLookupArray = new LookupArray*[m_Capacity];
7085  for (kt_int32u i = 0; i < m_Capacity; i++)
7086  {
7087  m_ppLookupArray[i] = new LookupArray();
7088  }
7089  }
7090  ar & boost::serialization::make_array<LookupArray*>(m_ppLookupArray, m_Capacity);
7091  }
7092  }; // class GridIndexLookup
7093 
7097 
7098  inline Pose2::Pose2(const Pose3& rPose)
7099  : m_Position(rPose.GetPosition().GetX(), rPose.GetPosition().GetY())
7100  {
7101  kt_double t1, t2;
7102 
7103  // calculates heading from orientation
7104  rPose.GetOrientation().ToEulerAngles(m_Heading, t1, t2);
7105  }
7106 
7110 
7111  // @cond EXCLUDE
7112 
7113  template<typename T>
7114  inline void Object::SetParameter(const std::string& rName, T value)
7115  {
7116  AbstractParameter* pParameter = GetParameter(rName);
7117  if (pParameter != NULL)
7118  {
7119  std::stringstream stream;
7120  stream << value;
7121  pParameter->SetValueFromString(stream.str());
7122  }
7123  else
7124  {
7125  throw Exception("Parameter does not exist: " + rName);
7126  }
7127  }
7128 
7129  template<>
7130  inline void Object::SetParameter(const std::string& rName, kt_bool value)
7131  {
7132  AbstractParameter* pParameter = GetParameter(rName);
7133  if (pParameter != NULL)
7134  {
7135  pParameter->SetValueFromString(value ? "true" : "false");
7136  }
7137  else
7138  {
7139  throw Exception("Parameter does not exist: " + rName);
7140  }
7141  }
7142 
7143  // @endcond
7144 
7146 }; // namespace karto
7147 
7175 
7176 #endif // karto_sdk_KARTO_H
void Add(const BoundingBox2 &rBoundingBox)
Definition: Karto.h:2934
BoundingBox2 m_BoundingBox
Definition: Karto.h:5845
const Vector3< kt_double > & GetPosition() const
Definition: Karto.h:2335
std::vector< kt_double > m_Angles
Definition: Karto.h:7073
Parameters(const std::string &rName)
Definition: Karto.h:3565
const kt_objecttype ObjectType_Misc
Definition: Karto.h:69
AbstractParameter * GetParameter(const std::string &rName) const
Definition: Karto.h:687
T GetHeight() const
Definition: Karto.h:1923
std::string ToString() const
Definition: Karto.h:467
const kt_objecttype ObjectType_CustomData
Definition: Karto.h:68
LaserRangeFinder * GetLaserRangeFinder() const
Definition: Karto.h:5395
#define NULL
std::string m_Description
Definition: Karto.h:3203
kt_double * GetRangeReadings() const
Definition: Karto.h:5338
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:5419
kt_int32u m_Capacity
Definition: Karto.h:7067
void SetSize(const Size2< kt_int32s > &rSize)
Definition: Karto.h:4566
void SetX(T x)
Definition: Karto.h:1878
kt_double operator()(kt_int32u row, kt_int32u column) const
Definition: Karto.h:2640
const Pose2 & GetBarycenterPose() const
Definition: Karto.h:5579
DrivePose(const Name &rSensorName)
Definition: Karto.h:5451
AbstractParameter(const std::string &rName, const std::string &rDescription, ParameterManager *pParameterManger=NULL)
Definition: Karto.h:3129
std::string ToString() const
Definition: Karto.h:1427
int32_t kt_int32s
Definition: Types.h:51
PointVectorDouble m_UnfilteredPointReadings
Definition: Karto.h:5840
Matrix3(const Matrix3 &rOther)
Definition: Karto.h:2456
virtual kt_bool Validate()
Definition: Karto.h:4150
Size2< kt_int32s > m_Size
Definition: Karto.h:4617
kt_int32s GetWidthStep() const
Definition: Karto.h:4868
kt_int32u GetNumberOfRangeReadings() const
Definition: Karto.h:4123
Matrix3 Inverse() const
Definition: Karto.h:2543
void SetSize(const Size2< T > &rSize)
Definition: Karto.h:1978
Quaternion m_Orientation
Definition: Karto.h:2432
const kt_objecttype ObjectType_LocalizedRangeScan
Definition: Karto.h:77
kt_double SquaredDistance(const Pose2 &rOther) const
Definition: Karto.h:2169
std::vector< Sensor * > SensorVector
Definition: Karto.h:3685
virtual ~Singleton()
Definition: Karto.h:223
void SetHeight(T height)
Definition: Karto.h:1932
virtual kt_objecttype GetObjectType() const =0
Exception(const std::string &rMessage="Karto Exception", kt_int32s errorCode=0)
Definition: Karto.h:105
CellUpdater * m_pCellUpdater
Definition: Karto.h:6417
Parameter< kt_double > * m_pOccupancyThreshold
Definition: Karto.h:6428
kt_bool IsSensorData(Object *pObject)
Definition: Karto.h:754
void SetY(const T &y)
Definition: Karto.h:1360
Pose3(const Vector3< kt_double > &rPosition)
Definition: Karto.h:2296
virtual ~Name()
Definition: Karto.h:416
kt_int32s GetDataSize() const
Definition: Karto.h:4895
Pose2(kt_double x, kt_double y, kt_double heading)
Definition: Karto.h:2072
virtual ~ParameterManager()
Definition: Karto.h:300
LaserRangeScan(const Name &rSensorName)
Definition: Karto.h:5298
const kt_int32s INVALID_SCAN
Definition: Math.h:47
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:983
void SetMinimum(const Vector2< kt_double > &mMinimum)
Definition: Karto.h:2891
Name(const std::string &rName)
Definition: Karto.h:400
kt_double Round(kt_double value)
Definition: Math.h:87
void MakeCeil(const Vector2 &rOther)
Definition: Karto.h:1072
kt_double & operator()(kt_int32u row, kt_int32u column)
Definition: Karto.h:2629
void SetUniqueId(kt_int32u uniqueId)
Definition: Karto.h:5170
Pose2 GetSensorPose() const
Definition: Karto.h:5621
const Vector2< kt_double > & GetOffset() const
Definition: Karto.h:4548
static void Validate(Sensor *pSensor)
Definition: Karto.h:3815
const T & GetX() const
Definition: Karto.h:1333
kt_int32s GetStateId() const
Definition: Karto.h:5143
const Pose2 & GetOdometricPose() const
Definition: Karto.h:5538
std::string ToString()
Definition: Karto.h:2371
void SaveToFile(const std::string &filename)
Definition: Karto.h:6549
kt_double GetScale() const
Definition: Karto.h:4530
virtual void Update()
Definition: Karto.h:5725
void SetBarycenterPose(Pose2 &bcenter)
Definition: Karto.h:5593
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1281
const kt_objecttype ObjectType_DatasetInfo
Definition: Karto.h:83
f
virtual ~CustomData()
Definition: Karto.h:5081
void SetValue(const T &rValue)
Definition: Karto.h:3280
const kt_objecttype ObjectType_CameraImage
Definition: Karto.h:78
void SetX(kt_double x)
Definition: Karto.h:1631
Pose2(const Vector2< kt_double > &rPosition, kt_double heading)
Definition: Karto.h:2060
Matrix3 Transpose() const
Definition: Karto.h:2525
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3209
kt_int32s GetWidth() const
Definition: Karto.h:4841
void FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll)
Definition: Karto.h:1734
Write
Pose2 m_Transform
Definition: Karto.h:3060
void DefineEnumValue(kt_int32s value, const std::string &rName)
Definition: Karto.h:3494
void SetWidth(T width)
Definition: Karto.h:917
void SetPosition(const T &rX, const T &rY)
Definition: Karto.h:1951
kt_double GetResolution() const
Definition: Karto.h:4584
kt_bool IsValidFirst(char c)
Definition: Karto.h:595
void SetRangeThreshold(kt_double rangeThreshold)
Definition: Karto.h:3992
const kt_objecttype ObjectType_Module
Definition: Karto.h:84
std::vector< Object * > ObjectVector
Definition: Karto.h:732
kt_int32u kt_objecttype
Definition: Karto.h:63
kt_int32s * GetArrayPointer()
Definition: Karto.h:6815
kt_int32u m_NumberOfRangeReadings
Definition: Karto.h:4396
T m_Values[2]
Definition: Karto.h:1275
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5560
kt_int32u GetNumberOfRangeReadings() const
Definition: Karto.h:5404
Vector2< kt_int32s > WorldToGrid(const Vector2< kt_double > &rWorld, kt_bool flipY=false) const
Definition: Karto.h:4799
const Vector2< kt_double > & GetPosition() const
Definition: Karto.h:2133
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:2956
Vector2< kt_double > m_Maximum
Definition: Karto.h:2964
uint8_t kt_int8u
Definition: Types.h:46
kt_bool IsLocalizedRangeScanWithPoints(Object *pObject)
Definition: Karto.h:784
const T & Maximum(const T &value1, const T &value2)
Definition: Math.h:111
Vector2< kt_int32s > WorldToGrid(const Vector2< kt_double > &rWorld, kt_bool flipY=false) const
Definition: Karto.h:4486
kt_double GetZ() const
Definition: Karto.h:1658
kt_double DegreesToRadians(kt_double degrees)
Definition: Math.h:56
void SetScope(const std::string &rScope)
Definition: Karto.h:458
const kt_objecttype ObjectType_None
Definition: Karto.h:65
AbstractParameter(const std::string &rName, ParameterManager *pParameterManger=NULL)
Definition: Karto.h:3113
const kt_objecttype ObjectType_Header
Definition: Karto.h:81
void SetX(const T &x)
Definition: Karto.h:1035
Parameter< kt_double > * m_pMinimumAngle
Definition: Karto.h:4382
virtual ~LaserRangeFinder()
Definition: Karto.h:3934
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:5262
kt_int32u GetSize() const
Definition: Karto.h:6761
void LoadFromFile(const std::string &filename)
Definition: Karto.h:6561
Grid< T > * m_pGrid
Definition: Karto.h:7065
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:6508
LaserRangeFinder(const Name &rName)
Definition: Karto.h:4332
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:2716
LocalizedRangeScanWithPoints(const Name &rSensorName, const RangeReadingsVector &rReadings, const PointVectorDouble &rPoints)
Definition: Karto.h:5877
virtual ~LookupArray()
Definition: Karto.h:6740
const PointVectorDouble & GetPointReadings(kt_bool wantFiltered=false) const
Definition: Karto.h:5687
Size2(const Size2 &rOther)
Definition: Karto.h:897
void SetY(T y)
Definition: Karto.h:1896
const T GetHeight() const
Definition: Karto.h:926
Size2< T > m_Size
Definition: Karto.h:2022
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:4401
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:196
Vector3(const Vector3 &rOther)
Definition: Karto.h:1321
Parameter(const std::string &rName, T value, ParameterManager *pParameterManger=NULL)
Definition: Karto.h:3237
void SetParameter(const std::string &rName, T value)
LaserRangeFinderType
Definition: Karto.h:3082
AbstractParameter * Get(const std::string &rName)
Definition: Karto.h:317
void SetStateId(kt_int32s stateId)
Definition: Karto.h:5152
std::map< kt_int32s, Object * > DataMap
Definition: Karto.h:733
void SetBoundingBox(BoundingBox2 &bbox)
Definition: Karto.h:5679
LaserRangeScan(const Name &rSensorName, const RangeReadingsVector &rRangeReadings)
Definition: Karto.h:5314
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:720
Parameter< Pose2 > * m_pOffsetPose
Definition: Karto.h:3672
Parameter(const std::string &rName, const std::string &rDescription, T value, ParameterManager *pParameterManger=NULL)
Definition: Karto.h:3250
virtual void UpdateCell(kt_int8u *pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
Definition: Karto.h:6343
const T & GetZ() const
Definition: Karto.h:1369
#define forEach(listtype, list)
Definition: Macros.h:66
kt_double * m_pData
Definition: Karto.h:2857
const kt_objecttype ObjectType_LaserRangeFinder
Definition: Karto.h:72
void SetW(kt_double w)
Definition: Karto.h:1685
boost::shared_mutex m_Lock
Definition: Karto.h:5531
kt_double GetResolution() const
Definition: Karto.h:4924
static void ComputeDimensions(const LocalizedRangeScanVector &rScans, kt_double resolution, kt_int32s &rWidth, kt_int32s &rHeight, Vector2< kt_double > &rOffset)
Definition: Karto.h:6183
virtual Grid< kt_int32u > * GetCellPassCounts()
Definition: Karto.h:6169
kt_double GetAngularResolution() const
Definition: Karto.h:4047
const PointVectorDouble m_Points
Definition: Karto.h:5955
Parameter< std::string > * m_pAuthor
Definition: Karto.h:6503
kt_double GetX() const
Definition: Karto.h:1622
void SetSensorPose(const Pose2 &rScanPose)
Definition: Karto.h:5635
virtual kt_double RayCast(const Pose2 &rPose2, kt_double maxRange) const
Definition: Karto.h:6096
void SetMinimumAngle(kt_double minimumAngle)
Definition: Karto.h:4016
Pose2 GetSensorAt(const Pose2 &rPose) const
Definition: Karto.h:5656
const std::string & GetName() const
Definition: Karto.h:425
void Add(const Vector2< kt_double > &rPoint)
Definition: Karto.h:2925
kt_int32u GetColumns() const
Definition: Karto.h:2778
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3342
ObjectVector m_Lasers
Definition: Karto.h:6692
kt_int32s GetUniqueId() const
Definition: Karto.h:5161
void SetMaximumRange(kt_double maximumRange)
Definition: Karto.h:3972
Drive(const std::string &rName)
Definition: Karto.h:3862
void SetOffsetPose(const Pose2 &rPose)
Definition: Karto.h:3639
void SetToIdentity()
Definition: Karto.h:2465
Sensor * GetSensorByName(const Name &rName)
Definition: Karto.h:3771
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
Parameter< std::string > * m_pDescription
Definition: Karto.h:6504
virtual const std::string GetValueAsString() const
Definition: Karto.h:3289
kt_int32s m_UniqueId
Definition: Karto.h:5246
Rectangle2(const Rectangle2 &rOther)
Definition: Karto.h:1858
virtual ~DatasetInfo()
Definition: Karto.h:6456
kt_int32s * GetArrayPointer() const
Definition: Karto.h:6824
kt_int32s m_Width
Definition: Karto.h:5028
Parameter< kt_double > * m_pAngularResolution
Definition: Karto.h:4385
kt_int32u m_Size
Definition: Karto.h:6832
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
Pose3(const Vector3< kt_double > &rPosition, const karto::Quaternion &rOrientation)
Definition: Karto.h:2306
void Clear()
Definition: Karto.h:4681
const Pose3 & GetOdometricPose() const
Definition: Karto.h:5468
const Size2< kt_int32s > & GetSize() const
Definition: Karto.h:4575
Vector2< kt_int32s > IndexToGrid(kt_int32s index) const
Definition: Karto.h:4783
Vector2< T > m_Position
Definition: Karto.h:2021
const kt_objecttype ObjectType_Sensor
Definition: Karto.h:66
void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2Vector &rLocalPoints, LocalizedRangeScan *pScan)
Definition: Karto.h:6968
void SetPosition(const Vector2< T > &rPosition)
Definition: Karto.h:1960
Pose2(const Pose2 &rOther)
Definition: Karto.h:2086
const Size2< kt_int32s > GetSize() const
Definition: Karto.h:4859
kt_double SquaredLength() const
Definition: Karto.h:1082
const std::vector< kt_double > & GetAngles() const
Definition: Karto.h:6910
kt_double SquaredLength() const
Definition: Karto.h:1409
kt_double Length() const
Definition: Karto.h:1091
kt_double GetY() const
Definition: Karto.h:1640
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:6835
void SetResolution(kt_double resolution)
Definition: Karto.h:4593
kt_double & operator()(kt_int32u row, kt_int32u column)
Definition: Karto.h:2789
kt_int32s m_StateId
Definition: Karto.h:5241
kt_int32s GetHeight() const
Definition: Karto.h:4850
virtual kt_bool AddScan(LocalizedRangeScan *pScan, kt_bool doUpdate=false)
Definition: Karto.h:6242
kt_int32s * m_pArray
Definition: Karto.h:6830
std::vector< kt_double > RangeReadingsVector
Definition: Karto.h:5281
void SetAngularResolution(kt_double angularResolution)
Definition: Karto.h:4056
const kt_objecttype ObjectType_Camera
Definition: Karto.h:73
const kt_objecttype ObjectType_SensorData
Definition: Karto.h:67
Vector2(T x, T y)
Definition: Karto.h:1015
Parameter< std::string > * m_pTitle
Definition: Karto.h:6502
Pose3(const Pose3 &rOther)
Definition: Karto.h:2315
void ComputeOffsets(LocalizedRangeScan *pScan, kt_double angleCenter, kt_double angleOffset, kt_double angleResolution)
Definition: Karto.h:6922
std::string m_Message
Definition: Karto.h:167
const Quaternion & GetOrientation() const
Definition: Karto.h:2353
virtual ~LaserRangeScan()
Definition: Karto.h:5327
std::string m_Scope
Definition: Karto.h:612
Pose3 m_OdometricPose
Definition: Karto.h:5490
Parameter< kt_int32u > * m_pMinPassThrough
Definition: Karto.h:6425
void SetScale(kt_double scale)
Definition: Karto.h:4539
kt_double SquaredDistance(const Vector2 &rOther) const
Definition: Karto.h:1100
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:2028
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:2254
const Size2< T > & GetSize() const
Definition: Karto.h:1969
const std::string & GetDescription() const
Definition: Karto.h:6480
kt_double GetRangeThreshold() const
Definition: Karto.h:3983
Quaternion(const Quaternion &rQuaternion)
Definition: Karto.h:1609
kt_double GetMaximumRange() const
Definition: Karto.h:3963
kt_int32s m_WidthStep
Definition: Karto.h:5030
kt_bool IsValidGridIndex(const Vector2< kt_int32s > &rGrid) const
Definition: Karto.h:4744
void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor *f=NULL)
Definition: Karto.h:4947
virtual kt_int32s GridIndex(const Vector2< kt_int32s > &rGrid, kt_bool boundaryCheck=true) const
Definition: Karto.h:4755
const Vector2< T > & GetPosition() const
Definition: Karto.h:1941
virtual kt_bool Validate()
Definition: Karto.h:3874
Quaternion(kt_double x, kt_double y, kt_double z, kt_double w)
Definition: Karto.h:1598
Vector2< kt_double > m_Position
Definition: Karto.h:2261
T m_Height
Definition: Karto.h:980
virtual ParameterManager * GetParameterManager()
Definition: Karto.h:677
Grid * Clone()
Definition: Karto.h:4690
const Name & GetSensorName() const
Definition: Karto.h:5197
void SetMaximumAngle(kt_double maximumAngle)
Definition: Karto.h:4036
T * m_pPointer
Definition: Karto.h:246
std::vector< CustomData * > CustomDataVector
Definition: Karto.h:5114
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3067
virtual void Update()
Definition: Karto.h:6363
const T & Clip(const T &n, const T &minValue, const T &maxValue)
Definition: Math.h:124
void Clear()
Definition: Karto.h:2478
virtual ~OccupancyGrid()
Definition: Karto.h:6021
virtual ~Parameters()
Definition: Karto.h:3573
void SetX(const T &x)
Definition: Karto.h:1342
kt_bool IsLaserRangeFinder(Object *pObject)
Definition: Karto.h:764
const DataMap & GetData() const
Definition: Karto.h:6618
virtual void Clear()
Definition: Karto.h:6655
virtual ~Parameter()
Definition: Karto.h:3262
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:4623
const ParameterVector & GetParameters() const
Definition: Karto.h:704
kt_double GetMinimumAngle() const
Definition: Karto.h:4007
void SetIs360Laser(bool is_360_laser)
Definition: Karto.h:4142
virtual void Resize(kt_int32s width, kt_int32s height)
Definition: Karto.h:6387
Name(const Name &rOther)
Definition: Karto.h:408
SensorVector GetAllSensors()
Definition: Karto.h:3798
ParameterManager * m_pParameterManager
Definition: Karto.h:714
#define KARTO_Object(name)
Definition: Karto.h:59
void SetOccupancyThreshold(kt_double thresh)
Definition: Karto.h:6150
virtual ~AbstractParameter()
Definition: Karto.h:3145
RangeReadingsVector GetRangeReadingsVector() const
Definition: Karto.h:5343
virtual void Resize(kt_int32s width, kt_int32s height)
Definition: Karto.h:4705
T * GetSensorByName(const Name &rName)
Definition: Karto.h:3787
virtual ~Dataset()
Definition: Karto.h:6540
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:5104
void MakeFloor(const Vector2 &rOther)
Definition: Karto.h:1062
kt_bool IsDatasetInfo(Object *pObject)
Definition: Karto.h:804
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
static Grid * CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
Definition: Karto.h:4653
void UnregisterSensor(Sensor *pSensor)
Definition: Karto.h:3750
std::map< std::string, kt_int32s > EnumMap
Definition: Karto.h:3411
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3536
virtual ~ParameterEnum()
Definition: Karto.h:3431
kt_bool GetIs360Laser() const
Definition: Karto.h:4133
BOOST_CLASS_EXPORT_KEY(karto::NonCopyable)
Parameter< kt_double > * m_pRangeThreshold
Definition: Karto.h:4390
virtual kt_bool RayTrace(const Vector2< kt_double > &rWorldFrom, const Vector2< kt_double > &rWorldTo, kt_bool isEndPointValid, kt_bool doUpdate=false)
Definition: Karto.h:6300
void SetPosition(const Vector3< kt_double > &rPosition)
Definition: Karto.h:2344
kt_double GetX() const
Definition: Karto.h:2097
Grid< kt_int32u > * m_pCellPassCnt
Definition: Karto.h:6398
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:368
Parameter< kt_double > * m_pMinimumRange
Definition: Karto.h:4387
std::vector< Pose2 > Pose2Vector
Definition: Karto.h:2269
kt_bool IsInBounds(const Vector2< kt_double > &rPoint) const
Definition: Karto.h:2945
virtual const std::string GetValueAsString() const =0
void SetOffset(const Vector2< kt_double > &rOffset)
Definition: Karto.h:4557
DataMap m_Data
Definition: Karto.h:6693
kt_int32s GetErrorCode()
Definition: Karto.h:153
Matrix3 m_Rotation
Definition: Karto.h:3062
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3675
const Vector2< T > GetCenter() const
Definition: Karto.h:1987
T * GetDataPointer()
Definition: Karto.h:4877
DatasetInfo * GetDatasetInfo()
Definition: Karto.h:6647
const T & GetValue() const
Definition: Karto.h:3271
virtual Parameter * Clone()
Definition: Karto.h:3311
const std::string & GetAuthor() const
Definition: Karto.h:6472
void ToEulerAngles(kt_double &rYaw, kt_double &rPitch, kt_double &rRoll) const
Definition: Karto.h:1697
std::string m_Name
Definition: Karto.h:3202
T GetWidth() const
Definition: Karto.h:1905
Rectangle2(const Vector2< T > &rPosition, const Size2< T > &rSize)
Definition: Karto.h:1849
T * GetDataPointer(const Vector2< kt_int32s > &rGrid)
Definition: Karto.h:4820
Size2(T width, T height)
Definition: Karto.h:887
kt_double * m_pRangeReadings
Definition: Karto.h:5414
T * GetDataPointer(const Vector2< kt_int32s > &rGrid) const
Definition: Karto.h:4831
std::map< std::string, AbstractParameter * > m_ParameterLookup
Definition: Karto.h:364
Name m_SensorName
Definition: Karto.h:5251
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
EnumMap m_EnumDefines
Definition: Karto.h:3532
kt_int32s m_Height
Definition: Karto.h:5029
virtual kt_bool IsFree(const Vector2< kt_int32s > &rPose) const
Definition: Karto.h:6078
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:172
kt_int32u GetRows() const
Definition: Karto.h:2769
const kt_double KT_TOLERANCE
Definition: Math.h:41
kt_double NormalizeAngle(kt_double angle)
Definition: Math.h:182
kt_int32u m_Size
Definition: Karto.h:7068
const T & GetX() const
Definition: Karto.h:1026
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:5037
kt_int32u m_NumberOfRangeReadings
Definition: Karto.h:5415
static SensorManager * GetInstance()
Definition: Karto.cpp:57
#define const_forEach(listtype, list)
Definition: Macros.h:72
Matrix(kt_int32u rows, kt_int32u columns)
Definition: Karto.h:2735
ParameterVector m_Parameters
Definition: Karto.h:363
virtual const std::string GetValueAsString() const
Definition: Karto.h:3476
static OccupancyGrid * CreateFromScans(const LocalizedRangeScanVector &rScans, kt_double resolution)
Definition: Karto.h:6038
kt_bool InverseFast(Matrix3 &rkInverse, kt_double fTolerance=KT_TOLERANCE) const
Definition: Karto.h:2558
CoordinateConverter * GetCoordinateConverter() const
Definition: Karto.h:4915
Parameter< kt_double > * m_pMaximumRange
Definition: Karto.h:4388
void SetSize(kt_int32u size)
Definition: Karto.h:7021
GridIndexLookup(Grid< T > *pGrid)
Definition: Karto.h:6877
virtual ~SensorManager()
Definition: Karto.h:3712
kt_int32s GetType()
Definition: Karto.h:4114
kt_bool IsParameters(Object *pObject)
Definition: Karto.h:794
Vector2< kt_double > m_Offset
Definition: Karto.h:4620
virtual void SetValueFromString(const std::string &rStringValue)=0
Pose2 TransformPose(const Pose2 &rSourcePose)
Definition: Karto.h:3002
Pose2 GetReferencePose(kt_bool useBarycenter) const
Definition: Karto.h:5603
BoundingBox2 GetBoundingBox() const
Definition: Karto.h:4602
void SetOdometricPose(const Pose2 &rPose)
Definition: Karto.h:5547
virtual void CreateFromScans(const LocalizedRangeScanVector &rScans)
Definition: Karto.h:6213
void Validate(const std::string &rName)
Definition: Karto.h:565
Parameter< kt_bool > * m_pIs360Laser
Definition: Karto.h:4392
virtual ~Grid()
Definition: Karto.h:4665
kt_double GetMaximumAngle() const
Definition: Karto.h:4027
void Clear()
Definition: Karto.h:2757
void SetHeight(T height)
Definition: Karto.h:935
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:7076
static LaserRangeFinder * CreateLaserRangeFinder(LaserRangeFinderType type, const Name &rName)
Definition: Karto.h:4185
bool kt_bool
Definition: Types.h:64
Vector2< kt_double > m_Minimum
Definition: Karto.h:2963
kt_double Distance(const Vector2 &rOther) const
Definition: Karto.h:1110
const kt_objecttype ObjectType_DrivePose
Definition: Karto.h:75
DatasetInfo * m_pDatasetInfo
Definition: Karto.h:6694
kt_double GetW() const
Definition: Karto.h:1676
kt_double GetTime() const
Definition: Karto.h:5179
const std::string & GetTitle() const
Definition: Karto.h:6464
Vector2< kt_double > GridToWorld(const Vector2< kt_int32s > &rGrid, kt_bool flipY=false) const
Definition: Karto.h:4509
virtual ~DrivePose()
Definition: Karto.h:5459
void SetName(const std::string &rName)
Definition: Karto.h:434
ParameterEnum * m_pType
Definition: Karto.h:4394
Size2< kt_double > GetSize() const
Definition: Karto.h:2915
virtual ~GridIndexLookup()
Definition: Karto.h:6888
Vector2< kt_double > GridToWorld(const Vector2< kt_int32s > &rGrid, kt_bool flipY=false) const
Definition: Karto.h:4810
T GetValue(const Vector2< kt_int32s > &rGrid) const
Definition: Karto.h:4905
virtual void SetValueFromString(const std::string &rStringValue)
Definition: Karto.h:3453
kt_int32s m_ErrorCode
Definition: Karto.h:168
virtual ~LocalizedRangeScan()
Definition: Karto.h:5526
LookupArray ** m_ppLookupArray
Definition: Karto.h:7070
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3894
void SetPosition(const Vector2< kt_double > &rPosition)
Definition: Karto.h:2142
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
void Parse(const std::string &rName)
Definition: Karto.h:540
virtual ~Drive()
Definition: