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 #include <mutex>
32 #include <shared_mutex>
33 
34 #include <math.h>
35 #include <float.h>
36 #include <stdio.h>
37 #include <assert.h>
38 #include <string.h>
39 #include <boost/thread.hpp>
40 
41 #include <boost/serialization/base_object.hpp>
42 #include <boost/serialization/nvp.hpp>
43 #include <boost/serialization/utility.hpp>
44 #include <boost/serialization/export.hpp>
45 #include <boost/type_traits/is_abstract.hpp>
46 #include <boost/archive/binary_iarchive.hpp>
47 #include <boost/archive/binary_oarchive.hpp>
48 #include <boost/serialization/map.hpp>
49 #include <boost/serialization/vector.hpp>
50 #include <boost/serialization/string.hpp>
51 #include <boost/serialization/array.hpp>
52 #include <boost/version.hpp>
53 
54 #ifdef USE_POCO
55 #include <Poco/Mutex.h>
56 #endif
57 
58 #include <karto_sdk/Math.h>
59 #include <karto_sdk/Macros.h>
60 
61 #define KARTO_Object(name) \
62  virtual const char* GetClassName() const { return #name; } \
63  virtual kt_objecttype GetObjectType() const { return ObjectType_##name; }
64 
66 
67 const kt_objecttype ObjectType_None = 0x00000000;
68 const kt_objecttype ObjectType_Sensor = 0x00001000;
71 const kt_objecttype ObjectType_Misc = 0x10000000;
72 
76 
82 
87 
88 namespace karto
89 {
90 
95 
100  {
101  public:
107  Exception(const std::string& rMessage = "Karto Exception", kt_int32s errorCode = 0)
108  : m_Message(rMessage)
109  , m_ErrorCode(errorCode)
110  {
111  }
112 
116  Exception(const Exception& rException)
117  : m_Message(rException.m_Message)
118  , m_ErrorCode(rException.m_ErrorCode)
119  {
120  }
121 
125  virtual ~Exception()
126  {
127  }
128 
129  public:
133  Exception& operator = (const Exception& rException)
134  {
135  m_Message = rException.m_Message;
136  m_ErrorCode = rException.m_ErrorCode;
137 
138  return *this;
139  }
140 
141  public:
146  const std::string& GetErrorMessage() const
147  {
148  return m_Message;
149  }
150 
156  {
157  return m_ErrorCode;
158  }
159 
160  public:
166  friend KARTO_EXPORT std::ostream& operator << (std::ostream& rStream, Exception& rException);
167 
168  private:
169  std::string m_Message;
171  }; // class Exception
172 
176 
182  {
183  private:
184  NonCopyable(const NonCopyable&) = delete;
185  const NonCopyable& operator=(const NonCopyable&) = delete;
186 
187  public:
189  {
190  }
191 
192  virtual ~NonCopyable()
193  {
194  }
195 
196  friend class boost::serialization::access;
197  template<class Archive>
198  void serialize(Archive &ar, const unsigned int version)
199  {
200  }
201  }; // class NonCopyable
202 
206 
210  template <class T>
211  class Singleton
212  {
213  public:
218  : m_pPointer(NULL)
219  {
220  }
221 
225  virtual ~Singleton()
226  {
227  delete m_pPointer;
228  }
229 
234  T* Get()
235  {
236 #ifdef USE_POCO
237  Poco::FastMutex::ScopedLock lock(m_Mutex);
238 #endif
239  if (m_pPointer == NULL)
240  {
241  m_pPointer = new T;
242  }
243 
244  return m_pPointer;
245  }
246 
247  private:
249 
250 #ifdef USE_POCO
251  Poco::FastMutex m_Mutex;
252 #endif
253 
254  private:
255  Singleton(const Singleton&);
256  const Singleton& operator=(const Singleton&);
257  };
258 
262 
267  {
268  public:
272  virtual void operator() (kt_int32u) {};
273  }; // Functor
274 
278 
280 
284  typedef std::vector<AbstractParameter*> ParameterVector;
285 
290  {
291  public:
296  {
297  }
298 
303  {
304  Clear();
305  }
306 
307  public:
312  void Add(AbstractParameter* pParameter);
313 
319  AbstractParameter* Get(const std::string& rName)
320  {
321  if (m_ParameterLookup.find(rName) != m_ParameterLookup.end())
322  {
323  return m_ParameterLookup[rName];
324  }
325 
326  std::cout << "Unknown parameter: " << rName << std::endl;
327 
328  return NULL;
329  }
330 
334  void Clear();
335 
340  inline const ParameterVector& GetParameterVector() const
341  {
342  return m_Parameters;
343  }
344 
345  public:
351  AbstractParameter* operator() (const std::string& rName)
352  {
353  return Get(rName);
354  }
355 
360  private:
362  const ParameterManager& operator=(const ParameterManager&);
363 
364  private:
366  std::map<std::string, AbstractParameter*> m_ParameterLookup;
367 
368  friend class boost::serialization::access;
369  template<class Archive>
370  void serialize(Archive &ar, const unsigned int version)
371  {
372  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NonCopyable);
373  ar & BOOST_SERIALIZATION_NVP(m_Parameters);
374  ar & BOOST_SERIALIZATION_NVP(m_ParameterLookup);
375  }
376 
377  }; // ParameterManager
378 
382 
383  // valid names
384  // 'Test' -- no scope
385  // '/Test' -- no scope will be parsed to 'Test'
386  // '/scope/Test' - 'scope' scope and 'Test' name
387  // '/scope/name/Test' - 'scope/name' scope and 'Test' name
388  //
389  class Name
390  {
391  public:
396  {
397  }
398 
402  Name(const std::string& rName)
403  {
404  Parse(rName);
405  }
406 
410  Name(const Name& rOther)
411  : m_Scope(rOther.m_Scope), m_Name(rOther.m_Name)
412  {
413  }
414 
418  virtual ~Name()
419  {
420  }
421 
422  public:
427  inline const std::string& GetName() const
428  {
429  return m_Name;
430  }
431 
436  inline void SetName(const std::string& rName)
437  {
438  std::string::size_type pos = rName.find_last_of('/');
439  if (pos != 0 && pos != std::string::npos)
440  {
441  throw Exception("Name can't contain a scope!");
442  }
443 
444  m_Name = rName;
445  }
446 
451  inline const std::string& GetScope() const
452  {
453  return m_Scope;
454  }
455 
460  inline void SetScope(const std::string& rScope)
461  {
462  m_Scope = rScope;
463  }
464 
469  inline std::string ToString() const
470  {
471  if (m_Scope.empty())
472  {
473  return m_Name;
474  }
475  else
476  {
477  std::string name;
478  name.append("/");
479  name.append(m_Scope);
480  name.append("/");
481  name.append(m_Name);
482 
483  return name;
484  }
485  }
486 
487  public:
491  Name& operator = (const Name& rOther)
492  {
493  if (&rOther != this)
494  {
495  m_Name = rOther.m_Name;
496  m_Scope = rOther.m_Scope;
497  }
498 
499  return *this;
500  }
501 
505  kt_bool operator == (const Name& rOther) const
506  {
507  return (m_Name == rOther.m_Name) && (m_Scope == rOther.m_Scope);
508  }
509 
513  kt_bool operator != (const Name& rOther) const
514  {
515  return !(*this == rOther);
516  }
517 
521  kt_bool operator < (const Name& rOther) const
522  {
523  return this->ToString() < rOther.ToString();
524  }
525 
531  friend inline std::ostream& operator << (std::ostream& rStream, const Name& rName)
532  {
533  rStream << rName.ToString();
534  return rStream;
535  }
536 
537  private:
542  void Parse(const std::string& rName)
543  {
544  std::string::size_type pos = rName.find_last_of('/');
545 
546  if (pos == std::string::npos)
547  {
548  m_Name = rName;
549  }
550  else
551  {
552  m_Scope = rName.substr(0, pos);
553  m_Name = rName.substr(pos+1, rName.size());
554 
555  // remove '/' from m_Scope if first!!
556  if (m_Scope.size() > 0 && m_Scope[0] == '/')
557  {
558  m_Scope = m_Scope.substr(1, m_Scope.size());
559  }
560  }
561  }
562 
567  void Validate(const std::string& rName)
568  {
569  if (rName.empty())
570  {
571  return;
572  }
573 
574  char c = rName[0];
575  if (IsValidFirst(c))
576  {
577  for (size_t i = 1; i < rName.size(); ++i)
578  {
579  c = rName[i];
580  if (!IsValid(c))
581  {
582  throw Exception("Invalid character in name. Valid characters must be within the ranges A-Z, a-z, 0-9, '/', '_' and '-'.");
583  }
584  }
585  }
586  else
587  {
588  throw Exception("Invalid first character in name. Valid characters must be within the ranges A-Z, a-z, and '/'.");
589  }
590  }
591 
597  inline kt_bool IsValidFirst(char c)
598  {
599  return (isalpha(c) || c == '/');
600  }
601 
607  inline kt_bool IsValid(char c)
608  {
609  return (isalnum(c) || c == '/' || c == '_' || c == '-');
610  }
611 
612  private:
613  std::string m_Name;
614  std::string m_Scope;
619  template<class Archive>
620  void serialize(Archive &ar, const unsigned int version)
621  {
622  ar & BOOST_SERIALIZATION_NVP(m_Name);
623  ar & BOOST_SERIALIZATION_NVP(m_Scope);
624  }
625  };
626 
630 
635  {
636  public:
640  Object();
641 
646  Object(const Name& rName);
647 
651  virtual ~Object();
652 
653  public:
658  inline const Name& GetName() const
659  {
660  return m_Name;
661  }
662 
667  virtual const char* GetClassName() const = 0;
668 
673  virtual kt_objecttype GetObjectType() const = 0;
674 
680  {
681  return m_pParameterManager;
682  }
683 
689  inline AbstractParameter* GetParameter(const std::string& rName) const
690  {
691  return m_pParameterManager->Get(rName);
692  }
693 
699  template<typename T>
700  inline void SetParameter(const std::string& rName, T value);
701 
706  inline const ParameterVector& GetParameters() const
707  {
708  return m_pParameterManager->GetParameterVector();
709  }
710 
711  Object(const Object&);
712  const Object& operator=(const Object&);
713 
714  private:
720  friend class boost::serialization::access;
721  template<class Archive>
722  void serialize(Archive &ar, const unsigned int version)
723  {
724  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NonCopyable);
725  ar & BOOST_SERIALIZATION_NVP(m_pParameterManager);
726  ar & BOOST_SERIALIZATION_NVP(m_Name);
727  }
728  };
729  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Object)
730 
731 
734  typedef std::vector<Object*> ObjectVector;
735  typedef std::map<kt_int32s, Object*> DataMap;
736 
740 
746  inline kt_bool IsSensor(Object* pObject)
747  {
748  return (pObject->GetObjectType() & ObjectType_Sensor) == ObjectType_Sensor;
749  }
750 
756  inline kt_bool IsSensorData(Object* pObject)
757  {
759  }
760 
767  {
769  }
770 
777  {
779  }
780 
787  {
789  }
790 
796  inline kt_bool IsParameters(Object* pObject)
797  {
799  }
800 
806  inline kt_bool IsDatasetInfo(Object* pObject)
807  {
809  }
810 
814 
818  class KARTO_EXPORT Module : public Object
819  {
820  public:
821  // @cond EXCLUDE
823  // @endcond
824 
825  public:
830  Module(const std::string& rName);
831 
835  virtual ~Module();
836 
837  public:
841  virtual void Reset() = 0;
842 
847  {
848  return false;
849  }
850 
851  private:
852  Module(const Module&);
853  const Module& operator=(const Module&);
854 
855  friend class boost::serialization::access;
856  template<class Archive>
857  void serialize(Archive &ar, const unsigned int version)
858  {
859  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
860  }
861  };
862  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Module)
863 
864 
865 
871  template<typename T>
872  class Size2
873  {
874  public:
879  : m_Width(0)
880  , m_Height(0)
881  {
882  }
883 
889  Size2(T width, T height)
890  : m_Width(width)
891  , m_Height(height)
892  {
893  }
894 
899  Size2(const Size2& rOther)
900  : m_Width(rOther.m_Width)
901  , m_Height(rOther.m_Height)
902  {
903  }
904 
905  public:
910  inline const T GetWidth() const
911  {
912  return m_Width;
913  }
914 
919  inline void SetWidth(T width)
920  {
921  m_Width = width;
922  }
923 
928  inline const T GetHeight() const
929  {
930  return m_Height;
931  }
932 
937  inline void SetHeight(T height)
938  {
939  m_Height = height;
940  }
941 
945  inline Size2& operator = (const Size2& rOther)
946  {
947  m_Width = rOther.m_Width;
948  m_Height = rOther.m_Height;
949 
950  return(*this);
951  }
952 
956  inline kt_bool operator == (const Size2& rOther) const
957  {
958  return (m_Width == rOther.m_Width && m_Height == rOther.m_Height);
959  }
960 
964  inline kt_bool operator != (const Size2& rOther) const
965  {
966  return (m_Width != rOther.m_Width || m_Height != rOther.m_Height);
967  }
968 
974  friend inline std::ostream& operator << (std::ostream& rStream, const Size2& rSize)
975  {
976  rStream << "(" << rSize.m_Width << ", " << rSize.m_Height << ")";
977  return rStream;
978  }
979 
980  private:
983  friend class boost::serialization::access;
984  template<class Archive>
985  void serialize(Archive &ar, const unsigned int version)
986  {
987  ar & BOOST_SERIALIZATION_NVP(m_Width);
988  ar & BOOST_SERIALIZATION_NVP(m_Height);
989  }
990  }; // Size2<T>
991 
995 
999  template<typename T>
1000  class Vector2
1001  {
1002  public:
1007  {
1008  m_Values[0] = 0;
1009  m_Values[1] = 0;
1010  }
1011 
1017  Vector2(T x, T y)
1018  {
1019  m_Values[0] = x;
1020  m_Values[1] = y;
1021  }
1022 
1023  public:
1028  inline const T& GetX() const
1029  {
1030  return m_Values[0];
1031  }
1032 
1037  inline void SetX(const T& x)
1038  {
1039  m_Values[0] = x;
1040  }
1041 
1046  inline const T& GetY() const
1047  {
1048  return m_Values[1];
1049  }
1050 
1055  inline void SetY(const T& y)
1056  {
1057  m_Values[1] = y;
1058  }
1059 
1064  inline void MakeFloor(const Vector2& rOther)
1065  {
1066  if ( rOther.m_Values[0] < m_Values[0] ) m_Values[0] = rOther.m_Values[0];
1067  if ( rOther.m_Values[1] < m_Values[1] ) m_Values[1] = rOther.m_Values[1];
1068  }
1069 
1074  inline void MakeCeil(const Vector2& rOther)
1075  {
1076  if ( rOther.m_Values[0] > m_Values[0] ) m_Values[0] = rOther.m_Values[0];
1077  if ( rOther.m_Values[1] > m_Values[1] ) m_Values[1] = rOther.m_Values[1];
1078  }
1079 
1084  inline kt_double SquaredLength() const
1085  {
1086  return math::Square(m_Values[0]) + math::Square(m_Values[1]);
1087  }
1088 
1093  inline kt_double Length() const
1094  {
1095  return sqrt(SquaredLength());
1096  }
1097 
1102  inline kt_double SquaredDistance(const Vector2& rOther) const
1103  {
1104  return (*this - rOther).SquaredLength();
1105  }
1106 
1112  inline kt_double Distance(const Vector2& rOther) const
1113  {
1114  return sqrt(SquaredDistance(rOther));
1115  }
1116 
1117  public:
1121  inline void operator += (const Vector2& rOther)
1122  {
1123  m_Values[0] += rOther.m_Values[0];
1124  m_Values[1] += rOther.m_Values[1];
1125  }
1126 
1130  inline void operator -= (const Vector2& rOther)
1131  {
1132  m_Values[0] -= rOther.m_Values[0];
1133  m_Values[1] -= rOther.m_Values[1];
1134  }
1135 
1141  inline const Vector2 operator + (const Vector2& rOther) const
1142  {
1143  return Vector2(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1]);
1144  }
1145 
1151  inline const Vector2 operator - (const Vector2& rOther) const
1152  {
1153  return Vector2(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1]);
1154  }
1155 
1160  inline void operator /= (T scalar)
1161  {
1162  m_Values[0] /= scalar;
1163  m_Values[1] /= scalar;
1164  }
1165 
1171  inline const Vector2 operator / (T scalar) const
1172  {
1173  return Vector2(m_Values[0] / scalar, m_Values[1] / scalar);
1174  }
1175 
1181  inline kt_double operator * (const Vector2& rOther) const
1182  {
1183  return m_Values[0] * rOther.m_Values[0] + m_Values[1] * rOther.m_Values[1];
1184  }
1185 
1190  inline const Vector2 operator * (T scalar) const
1191  {
1192  return Vector2(m_Values[0] * scalar, m_Values[1] * scalar);
1193  }
1194 
1199  inline const Vector2 operator - (T scalar) const
1200  {
1201  return Vector2(m_Values[0] - scalar, m_Values[1] - scalar);
1202  }
1203 
1208  inline void operator *= (T scalar)
1209  {
1210  m_Values[0] *= scalar;
1211  m_Values[1] *= scalar;
1212  }
1213 
1218  inline kt_bool operator == (const Vector2& rOther) const
1219  {
1220  return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1]);
1221  }
1222 
1227  inline kt_bool operator != (const Vector2& rOther) const
1228  {
1229  return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1]);
1230  }
1231 
1237  inline kt_bool operator < (const Vector2& rOther) const
1238  {
1239  if (m_Values[0] < rOther.m_Values[0])
1240  return true;
1241  else if (m_Values[0] > rOther.m_Values[0])
1242  return false;
1243  else
1244  return (m_Values[1] < rOther.m_Values[1]);
1245  }
1246 
1252  friend inline std::ostream& operator << (std::ostream& rStream, const Vector2& rVector)
1253  {
1254  rStream << rVector.GetX() << " " << rVector.GetY();
1255  return rStream;
1256  }
1257 
1262  friend inline std::istream& operator >> (std::istream& rStream, const Vector2& /*rVector*/)
1263  {
1264  // Implement me!! TODO(lucbettaieb): What the what? Do I need to implement this?
1265  return rStream;
1266  }
1267 
1268  friend class boost::serialization::access;
1269  template<class Archive>
1270  void serialize(Archive &ar, const unsigned int version)
1271  {
1272  ar & boost::serialization::make_nvp("m_Values_0", m_Values[0]);
1273  ar & boost::serialization::make_nvp("m_Values_1", m_Values[1]);
1274  }
1275 
1276  private:
1277  T m_Values[2];
1278  }; // Vector2<T>
1279 
1283  typedef std::vector< Vector2<kt_double> > PointVectorDouble;
1284 
1288 
1292  template<typename T>
1293  class Vector3
1294  {
1295  public:
1300  {
1301  m_Values[0] = 0;
1302  m_Values[1] = 0;
1303  m_Values[2] = 0;
1304  }
1305 
1312  Vector3(T x, T y, T z)
1313  {
1314  m_Values[0] = x;
1315  m_Values[1] = y;
1316  m_Values[2] = z;
1317  }
1318 
1323  Vector3(const Vector3& rOther)
1324  {
1325  m_Values[0] = rOther.m_Values[0];
1326  m_Values[1] = rOther.m_Values[1];
1327  m_Values[2] = rOther.m_Values[2];
1328  }
1329 
1330  public:
1335  inline const T& GetX() const
1336  {
1337  return m_Values[0];
1338  }
1339 
1344  inline void SetX(const T& x)
1345  {
1346  m_Values[0] = x;
1347  }
1348 
1353  inline const T& GetY() const
1354  {
1355  return m_Values[1];
1356  }
1357 
1362  inline void SetY(const T& y)
1363  {
1364  m_Values[1] = y;
1365  }
1366 
1371  inline const T& GetZ() const
1372  {
1373  return m_Values[2];
1374  }
1375 
1380  inline void SetZ(const T& z)
1381  {
1382  m_Values[2] = z;
1383  }
1384 
1389  inline void MakeFloor(const Vector3& rOther)
1390  {
1391  if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
1392  if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
1393  if (rOther.m_Values[2] < m_Values[2]) m_Values[2] = rOther.m_Values[2];
1394  }
1395 
1400  inline void MakeCeil(const Vector3& rOther)
1401  {
1402  if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0];
1403  if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1];
1404  if (rOther.m_Values[2] > m_Values[2]) m_Values[2] = rOther.m_Values[2];
1405  }
1406 
1411  inline kt_double SquaredLength() const
1412  {
1414  }
1415 
1420  inline kt_double Length() const
1421  {
1422  return sqrt(SquaredLength());
1423  }
1424 
1429  inline std::string ToString() const
1430  {
1431  std::stringstream converter;
1432  converter.precision(std::numeric_limits<double>::digits10);
1433 
1434  converter << GetX() << " " << GetY() << " " << GetZ();
1435 
1436  return converter.str();
1437  }
1438 
1439  public:
1443  inline Vector3& operator = (const Vector3& rOther)
1444  {
1445  m_Values[0] = rOther.m_Values[0];
1446  m_Values[1] = rOther.m_Values[1];
1447  m_Values[2] = rOther.m_Values[2];
1448 
1449  return *this;
1450  }
1451 
1457  inline const Vector3 operator + (const Vector3& rOther) const
1458  {
1459  return Vector3(m_Values[0] + rOther.m_Values[0],
1460  m_Values[1] + rOther.m_Values[1],
1461  m_Values[2] + rOther.m_Values[2]);
1462  }
1463 
1469  inline const Vector3 operator + (kt_double scalar) const
1470  {
1471  return Vector3(m_Values[0] + scalar,
1472  m_Values[1] + scalar,
1473  m_Values[2] + scalar);
1474  }
1475 
1481  inline const Vector3 operator - (const Vector3& rOther) const
1482  {
1483  return Vector3(m_Values[0] - rOther.m_Values[0],
1484  m_Values[1] - rOther.m_Values[1],
1485  m_Values[2] - rOther.m_Values[2]);
1486  }
1487 
1493  inline const Vector3 operator - (kt_double scalar) const
1494  {
1495  return Vector3(m_Values[0] - scalar, m_Values[1] - scalar, m_Values[2] - scalar);
1496  }
1497 
1502  inline const Vector3 operator * (T scalar) const
1503  {
1504  return Vector3(m_Values[0] * scalar, m_Values[1] * scalar, m_Values[2] * scalar);
1505  }
1506 
1511  inline kt_bool operator == (const Vector3& rOther) const
1512  {
1513  return (m_Values[0] == rOther.m_Values[0] &&
1514  m_Values[1] == rOther.m_Values[1] &&
1515  m_Values[2] == rOther.m_Values[2]);
1516  }
1517 
1522  inline kt_bool operator != (const Vector3& rOther) const
1523  {
1524  return (m_Values[0] != rOther.m_Values[0] ||
1525  m_Values[1] != rOther.m_Values[1] ||
1526  m_Values[2] != rOther.m_Values[2]);
1527  }
1528 
1534  friend inline std::ostream& operator << (std::ostream& rStream, const Vector3& rVector)
1535  {
1536  rStream << rVector.ToString();
1537  return rStream;
1538  }
1539 
1544  friend inline std::istream& operator >> (std::istream& rStream, const Vector3& /*rVector*/)
1545  {
1546  // Implement me!!
1547  return rStream;
1548  }
1549 
1550  private:
1551  T m_Values[3];
1552  }; // Vector3
1553 
1557 
1580  {
1581  public:
1585  inline Quaternion()
1586  {
1587  m_Values[0] = 0.0;
1588  m_Values[1] = 0.0;
1589  m_Values[2] = 0.0;
1590  m_Values[3] = 1.0;
1591  }
1592 
1601  {
1602  m_Values[0] = x;
1603  m_Values[1] = y;
1604  m_Values[2] = z;
1605  m_Values[3] = w;
1606  }
1607 
1611  inline Quaternion(const Quaternion& rQuaternion)
1612  {
1613  m_Values[0] = rQuaternion.m_Values[0];
1614  m_Values[1] = rQuaternion.m_Values[1];
1615  m_Values[2] = rQuaternion.m_Values[2];
1616  m_Values[3] = rQuaternion.m_Values[3];
1617  }
1618 
1619  public:
1624  inline kt_double GetX() const
1625  {
1626  return m_Values[0];
1627  }
1628 
1633  inline void SetX(kt_double x)
1634  {
1635  m_Values[0] = x;
1636  }
1637 
1642  inline kt_double GetY() const
1643  {
1644  return m_Values[1];
1645  }
1646 
1651  inline void SetY(kt_double y)
1652  {
1653  m_Values[1] = y;
1654  }
1655 
1660  inline kt_double GetZ() const
1661  {
1662  return m_Values[2];
1663  }
1664 
1669  inline void SetZ(kt_double z)
1670  {
1671  m_Values[2] = z;
1672  }
1673 
1678  inline kt_double GetW() const
1679  {
1680  return m_Values[3];
1681  }
1682 
1687  inline void SetW(kt_double w)
1688  {
1689  m_Values[3] = w;
1690  }
1691 
1699  void ToEulerAngles(kt_double& rYaw, kt_double& rPitch, kt_double& rRoll) const
1700  {
1701  kt_double test = m_Values[0] * m_Values[1] + m_Values[2] * m_Values[3];
1702 
1703  if (test > 0.499)
1704  {
1705  // singularity at north pole
1706  rYaw = 2 * atan2(m_Values[0], m_Values[3]);;
1707  rPitch = KT_PI_2;
1708  rRoll = 0;
1709  }
1710  else if (test < -0.499)
1711  {
1712  // singularity at south pole
1713  rYaw = -2 * atan2(m_Values[0], m_Values[3]);
1714  rPitch = -KT_PI_2;
1715  rRoll = 0;
1716  }
1717  else
1718  {
1719  kt_double sqx = m_Values[0] * m_Values[0];
1720  kt_double sqy = m_Values[1] * m_Values[1];
1721  kt_double sqz = m_Values[2] * m_Values[2];
1722 
1723  rYaw = atan2(2 * m_Values[1] * m_Values[3] - 2 * m_Values[0] * m_Values[2], 1 - 2 * sqy - 2 * sqz);
1724  rPitch = asin(2 * test);
1725  rRoll = atan2(2 * m_Values[0] * m_Values[3] - 2 * m_Values[1] * m_Values[2], 1 - 2 * sqx - 2 * sqz);
1726  }
1727  }
1728 
1737  {
1738  kt_double angle;
1739 
1740  angle = yaw * 0.5;
1741  kt_double cYaw = cos(angle);
1742  kt_double sYaw = sin(angle);
1743 
1744  angle = pitch * 0.5;
1745  kt_double cPitch = cos(angle);
1746  kt_double sPitch = sin(angle);
1747 
1748  angle = roll * 0.5;
1749  kt_double cRoll = cos(angle);
1750  kt_double sRoll = sin(angle);
1751 
1752  m_Values[0] = sYaw * sPitch * cRoll + cYaw * cPitch * sRoll;
1753  m_Values[1] = sYaw * cPitch * cRoll + cYaw * sPitch * sRoll;
1754  m_Values[2] = cYaw * sPitch * cRoll - sYaw * cPitch * sRoll;
1755  m_Values[3] = cYaw * cPitch * cRoll - sYaw * sPitch * sRoll;
1756  }
1757 
1762  inline Quaternion& operator = (const Quaternion& rQuaternion)
1763  {
1764  m_Values[0] = rQuaternion.m_Values[0];
1765  m_Values[1] = rQuaternion.m_Values[1];
1766  m_Values[2] = rQuaternion.m_Values[2];
1767  m_Values[3] = rQuaternion.m_Values[3];
1768 
1769  return(*this);
1770  }
1771 
1776  inline kt_bool operator == (const Quaternion& rOther) const
1777  {
1778  return (m_Values[0] == rOther.m_Values[0] &&
1779  m_Values[1] == rOther.m_Values[1] &&
1780  m_Values[2] == rOther.m_Values[2] &&
1781  m_Values[3] == rOther.m_Values[3]);
1782  }
1783 
1788  inline kt_bool operator != (const Quaternion& rOther) const
1789  {
1790  return (m_Values[0] != rOther.m_Values[0] ||
1791  m_Values[1] != rOther.m_Values[1] ||
1792  m_Values[2] != rOther.m_Values[2] ||
1793  m_Values[3] != rOther.m_Values[3]);
1794  }
1795 
1801  friend inline std::ostream& operator << (std::ostream& rStream, const Quaternion& rQuaternion)
1802  {
1803  rStream << rQuaternion.m_Values[0] << " "
1804  << rQuaternion.m_Values[1] << " "
1805  << rQuaternion.m_Values[2] << " "
1806  << rQuaternion.m_Values[3];
1807  return rStream;
1808  }
1809 
1810  private:
1812  };
1813 
1817 
1822  template<typename T>
1824  {
1825  public:
1830  {
1831  }
1832 
1840  Rectangle2(T x, T y, T width, T height)
1841  : m_Position(x, y)
1842  , m_Size(width, height)
1843  {
1844  }
1845 
1851  Rectangle2(const Vector2<T>& rPosition, const Size2<T>& rSize)
1852  : m_Position(rPosition)
1853  , m_Size(rSize)
1854  {
1855  }
1856 
1860  Rectangle2(const Rectangle2& rOther)
1861  : m_Position(rOther.m_Position)
1862  , m_Size(rOther.m_Size)
1863  {
1864  }
1865 
1866  public:
1871  inline T GetX() const
1872  {
1873  return m_Position.GetX();
1874  }
1875 
1880  inline void SetX(T x)
1881  {
1882  m_Position.SetX(x);
1883  }
1884 
1889  inline T GetY() const
1890  {
1891  return m_Position.GetY();
1892  }
1893 
1898  inline void SetY(T y)
1899  {
1900  m_Position.SetY(y);
1901  }
1902 
1907  inline T GetWidth() const
1908  {
1909  return m_Size.GetWidth();
1910  }
1911 
1916  inline void SetWidth(T width)
1917  {
1918  m_Size.SetWidth(width);
1919  }
1920 
1925  inline T GetHeight() const
1926  {
1927  return m_Size.GetHeight();
1928  }
1929 
1934  inline void SetHeight(T height)
1935  {
1936  m_Size.SetHeight(height);
1937  }
1938 
1943  inline const Vector2<T>& GetPosition() const
1944  {
1945  return m_Position;
1946  }
1947 
1953  inline void SetPosition(const T& rX, const T& rY)
1954  {
1955  m_Position = Vector2<T>(rX, rY);
1956  }
1957 
1962  inline void SetPosition(const Vector2<T>& rPosition)
1963  {
1964  m_Position = rPosition;
1965  }
1966 
1971  inline const Size2<T>& GetSize() const
1972  {
1973  return m_Size;
1974  }
1975 
1980  inline void SetSize(const Size2<T>& rSize)
1981  {
1982  m_Size = rSize;
1983  }
1984 
1989  inline const Vector2<T> GetCenter() const
1990  {
1991  return Vector2<T>(m_Position.GetX() + m_Size.GetWidth() * 0.5, m_Position.GetY() + m_Size.GetHeight() * 0.5);
1992  }
1993 
1994  public:
1999  {
2000  m_Position = rOther.m_Position;
2001  m_Size = rOther.m_Size;
2002 
2003  return *this;
2004  }
2005 
2009  inline kt_bool operator == (const Rectangle2& rOther) const
2010  {
2011  return (m_Position == rOther.m_Position && m_Size == rOther.m_Size);
2012  }
2013 
2017  inline kt_bool operator != (const Rectangle2& rOther) const
2018  {
2019  return (m_Position != rOther.m_Position || m_Size != rOther.m_Size);
2020  }
2021 
2022  private:
2028  friend class boost::serialization::access;
2029  template<class Archive>
2030  void serialize(Archive &ar, const unsigned int version)
2031  {
2032  ar & BOOST_SERIALIZATION_NVP(m_Position);
2033  ar & BOOST_SERIALIZATION_NVP(m_Size);
2034  }
2035  }; // Rectangle2
2036 
2040 
2041  class Pose3;
2042 
2046  class Pose2
2047  {
2048  public:
2053  : m_Heading(0.0)
2054  {
2055  }
2056 
2062  Pose2(const Vector2<kt_double>& rPosition, kt_double heading)
2063  : m_Position(rPosition)
2064  , m_Heading(heading)
2065  {
2066  }
2067 
2075  : m_Position(x, y)
2076  , m_Heading(heading)
2077  {
2078  }
2079 
2083  Pose2(const Pose3& rPose);
2084 
2088  Pose2(const Pose2& rOther)
2089  : m_Position(rOther.m_Position)
2090  , m_Heading(rOther.m_Heading)
2091  {
2092  }
2093 
2094  public:
2099  inline kt_double GetX() const
2100  {
2101  return m_Position.GetX();
2102  }
2103 
2108  inline void SetX(kt_double x)
2109  {
2110  m_Position.SetX(x);
2111  }
2112 
2117  inline kt_double GetY() const
2118  {
2119  return m_Position.GetY();
2120  }
2121 
2126  inline void SetY(kt_double y)
2127  {
2128  m_Position.SetY(y);
2129  }
2130 
2135  inline const Vector2<kt_double>& GetPosition() const
2136  {
2137  return m_Position;
2138  }
2139 
2144  inline void SetPosition(const Vector2<kt_double>& rPosition)
2145  {
2146  m_Position = rPosition;
2147  }
2148 
2153  inline kt_double GetHeading() const
2154  {
2155  return m_Heading;
2156  }
2157 
2162  inline void SetHeading(kt_double heading)
2163  {
2164  m_Heading = heading;
2165  }
2166 
2171  inline kt_double SquaredDistance(const Pose2& rOther) const
2172  {
2173  return m_Position.SquaredDistance(rOther.m_Position);
2174  }
2175 
2176  public:
2180  inline Pose2& operator = (const Pose2& rOther)
2181  {
2182  m_Position = rOther.m_Position;
2183  m_Heading = rOther.m_Heading;
2184 
2185  return *this;
2186  }
2187 
2191  inline kt_bool operator == (const Pose2& rOther) const
2192  {
2193  return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading);
2194  }
2195 
2199  inline kt_bool operator != (const Pose2& rOther) const
2200  {
2201  return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading);
2202  }
2203 
2207  inline void operator += (const Pose2& rOther)
2208  {
2209  m_Position += rOther.m_Position;
2211  }
2212 
2218  inline Pose2 operator + (const Pose2& rOther) const
2219  {
2220  return Pose2(m_Position + rOther.m_Position, math::NormalizeAngle(m_Heading + rOther.m_Heading));
2221  }
2222 
2228  inline Pose2 operator - (const Pose2& rOther) const
2229  {
2230  return Pose2(m_Position - rOther.m_Position, math::NormalizeAngle(m_Heading - rOther.m_Heading));
2231  }
2232 
2237  friend inline std::istream& operator >> (std::istream& rStream, const Pose2& /*rPose*/)
2238  {
2239  // Implement me!!
2240  return rStream;
2241  }
2242 
2248  friend inline std::ostream& operator << (std::ostream& rStream, const Pose2& rPose)
2249  {
2250  rStream << rPose.m_Position.GetX() << " " << rPose.m_Position.GetY() << " " << rPose.m_Heading;
2251  return rStream;
2252  }
2253 
2255  template<class Archive>
2256  void serialize(Archive &ar, const unsigned int version)
2257  {
2258  ar & BOOST_SERIALIZATION_NVP(m_Position);
2259  ar & BOOST_SERIALIZATION_NVP(m_Heading);
2260  }
2261 
2262  private:
2264 
2266  }; // Pose2
2267 
2271  typedef std::vector< Pose2 > Pose2Vector;
2272 
2276 
2284  class Pose3
2285  {
2286  public:
2291  {
2292  }
2293 
2298  Pose3(const Vector3<kt_double>& rPosition)
2299  : m_Position(rPosition)
2300  {
2301  }
2302 
2308  Pose3(const Vector3<kt_double>& rPosition, const karto::Quaternion& rOrientation)
2309  : m_Position(rPosition)
2310  , m_Orientation(rOrientation)
2311  {
2312  }
2313 
2317  Pose3(const Pose3& rOther)
2318  : m_Position(rOther.m_Position)
2319  , m_Orientation(rOther.m_Orientation)
2320  {
2321  }
2322 
2326  Pose3(const Pose2& rPose)
2327  {
2328  m_Position = Vector3<kt_double>(rPose.GetX(), rPose.GetY(), 0.0);
2329  m_Orientation.FromEulerAngles(rPose.GetHeading(), 0.0, 0.0);
2330  }
2331 
2332  public:
2337  inline const Vector3<kt_double>& GetPosition() const
2338  {
2339  return m_Position;
2340  }
2341 
2346  inline void SetPosition(const Vector3<kt_double>& rPosition)
2347  {
2348  m_Position = rPosition;
2349  }
2350 
2355  inline const Quaternion& GetOrientation() const
2356  {
2357  return m_Orientation;
2358  }
2359 
2364  inline void SetOrientation(const Quaternion& rOrientation)
2365  {
2366  m_Orientation = rOrientation;
2367  }
2368 
2373  inline std::string ToString()
2374  {
2375  std::stringstream converter;
2376  converter.precision(std::numeric_limits<double>::digits10);
2377 
2378  converter << GetPosition() << " " << GetOrientation();
2379 
2380  return converter.str();
2381  }
2382 
2383  public:
2387  inline Pose3& operator = (const Pose3& rOther)
2388  {
2389  m_Position = rOther.m_Position;
2390  m_Orientation = rOther.m_Orientation;
2391 
2392  return *this;
2393  }
2394 
2398  inline kt_bool operator == (const Pose3& rOther) const
2399  {
2400  return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation);
2401  }
2402 
2406  inline kt_bool operator != (const Pose3& rOther) const
2407  {
2408  return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation);
2409  }
2410 
2416  friend inline std::ostream& operator << (std::ostream& rStream, const Pose3& rPose)
2417  {
2418  rStream << rPose.GetPosition() << ", " << rPose.GetOrientation();
2419  return rStream;
2420  }
2421 
2426  friend inline std::istream& operator >> (std::istream& rStream, const Pose3& /*rPose*/)
2427  {
2428  // Implement me!!
2429  return rStream;
2430  }
2431 
2432  private:
2435  }; // Pose3
2436 
2440 
2444  class Matrix3
2445  {
2446  public:
2451  {
2452  Clear();
2453  }
2454 
2458  inline Matrix3(const Matrix3& rOther)
2459  {
2460  memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
2461  }
2462 
2463  public:
2468  {
2469  memset(m_Matrix, 0, 9*sizeof(kt_double));
2470 
2471  for (kt_int32s i = 0; i < 3; i++)
2472  {
2473  m_Matrix[i][i] = 1.0;
2474  }
2475  }
2476 
2480  void Clear()
2481  {
2482  memset(m_Matrix, 0, 9*sizeof(kt_double));
2483  }
2484 
2492  void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
2493  {
2494  kt_double cosRadians = cos(radians);
2495  kt_double sinRadians = sin(radians);
2496  kt_double oneMinusCos = 1.0 - cosRadians;
2497 
2498  kt_double xx = x * x;
2499  kt_double yy = y * y;
2500  kt_double zz = z * z;
2501 
2502  kt_double xyMCos = x * y * oneMinusCos;
2503  kt_double xzMCos = x * z * oneMinusCos;
2504  kt_double yzMCos = y * z * oneMinusCos;
2505 
2506  kt_double xSin = x * sinRadians;
2507  kt_double ySin = y * sinRadians;
2508  kt_double zSin = z * sinRadians;
2509 
2510  m_Matrix[0][0] = xx * oneMinusCos + cosRadians;
2511  m_Matrix[0][1] = xyMCos - zSin;
2512  m_Matrix[0][2] = xzMCos + ySin;
2513 
2514  m_Matrix[1][0] = xyMCos + zSin;
2515  m_Matrix[1][1] = yy * oneMinusCos + cosRadians;
2516  m_Matrix[1][2] = yzMCos - xSin;
2517 
2518  m_Matrix[2][0] = xzMCos - ySin;
2519  m_Matrix[2][1] = yzMCos + xSin;
2520  m_Matrix[2][2] = zz * oneMinusCos + cosRadians;
2521  }
2522 
2528  {
2529  Matrix3 transpose;
2530 
2531  for (kt_int32u row = 0; row < 3; row++)
2532  {
2533  for (kt_int32u col = 0; col < 3; col++)
2534  {
2535  transpose.m_Matrix[row][col] = m_Matrix[col][row];
2536  }
2537  }
2538 
2539  return transpose;
2540  }
2541 
2546  {
2547  Matrix3 kInverse = *this;
2548  kt_bool haveInverse = InverseFast(kInverse, 1e-14);
2549  if (haveInverse == false)
2550  {
2551  assert(false);
2552  }
2553  return kInverse;
2554  }
2555 
2560  kt_bool InverseFast(Matrix3& rkInverse, kt_double fTolerance = KT_TOLERANCE) const
2561  {
2562  // Invert a 3x3 using cofactors. This is about 8 times faster than
2563  // the Numerical Recipes code which uses Gaussian elimination.
2564  rkInverse.m_Matrix[0][0] = m_Matrix[1][1]*m_Matrix[2][2] - m_Matrix[1][2]*m_Matrix[2][1];
2565  rkInverse.m_Matrix[0][1] = m_Matrix[0][2]*m_Matrix[2][1] - m_Matrix[0][1]*m_Matrix[2][2];
2566  rkInverse.m_Matrix[0][2] = m_Matrix[0][1]*m_Matrix[1][2] - m_Matrix[0][2]*m_Matrix[1][1];
2567  rkInverse.m_Matrix[1][0] = m_Matrix[1][2]*m_Matrix[2][0] - m_Matrix[1][0]*m_Matrix[2][2];
2568  rkInverse.m_Matrix[1][1] = m_Matrix[0][0]*m_Matrix[2][2] - m_Matrix[0][2]*m_Matrix[2][0];
2569  rkInverse.m_Matrix[1][2] = m_Matrix[0][2]*m_Matrix[1][0] - m_Matrix[0][0]*m_Matrix[1][2];
2570  rkInverse.m_Matrix[2][0] = m_Matrix[1][0]*m_Matrix[2][1] - m_Matrix[1][1]*m_Matrix[2][0];
2571  rkInverse.m_Matrix[2][1] = m_Matrix[0][1]*m_Matrix[2][0] - m_Matrix[0][0]*m_Matrix[2][1];
2572  rkInverse.m_Matrix[2][2] = m_Matrix[0][0]*m_Matrix[1][1] - m_Matrix[0][1]*m_Matrix[1][0];
2573 
2574  kt_double fDet = m_Matrix[0][0]*rkInverse.m_Matrix[0][0] +
2575  m_Matrix[0][1]*rkInverse.m_Matrix[1][0] +
2576  m_Matrix[0][2]*rkInverse.m_Matrix[2][0];
2577 
2578  if (fabs(fDet) <= fTolerance)
2579  {
2580  return false;
2581  }
2582 
2583  kt_double fInvDet = 1.0/fDet;
2584  for (size_t row = 0; row < 3; row++)
2585  {
2586  for (size_t col = 0; col < 3; col++)
2587  {
2588  rkInverse.m_Matrix[row][col] *= fInvDet;
2589  }
2590  }
2591 
2592  return true;
2593  }
2594 
2599  inline std::string ToString() const
2600  {
2601  std::stringstream converter;
2602  converter.precision(std::numeric_limits<double>::digits10);
2603 
2604  for (int row = 0; row < 3; row++)
2605  {
2606  for (int col = 0; col < 3; col++)
2607  {
2608  converter << m_Matrix[row][col] << " ";
2609  }
2610  }
2611 
2612  return converter.str();
2613  }
2614 
2615  public:
2619  inline Matrix3& operator = (const Matrix3& rOther)
2620  {
2621  memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
2622  return *this;
2623  }
2624 
2632  {
2633  return m_Matrix[row][column];
2634  }
2635 
2642  inline kt_double operator()(kt_int32u row, kt_int32u column) const
2643  {
2644  return m_Matrix[row][column];
2645  }
2646 
2652  Matrix3 operator * (const Matrix3& rOther) const
2653  {
2654  Matrix3 product;
2655 
2656  for (size_t row = 0; row < 3; row++)
2657  {
2658  for (size_t col = 0; col < 3; col++)
2659  {
2660  product.m_Matrix[row][col] = m_Matrix[row][0]*rOther.m_Matrix[0][col] +
2661  m_Matrix[row][1]*rOther.m_Matrix[1][col] +
2662  m_Matrix[row][2]*rOther.m_Matrix[2][col];
2663  }
2664  }
2665 
2666  return product;
2667  }
2668 
2674  inline Pose2 operator * (const Pose2& rPose2) const
2675  {
2676  Pose2 pose2;
2677 
2678  pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] *
2679  rPose2.GetY() + m_Matrix[0][2] * rPose2.GetHeading());
2680  pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] *
2681  rPose2.GetY() + m_Matrix[1][2] * rPose2.GetHeading());
2682  pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() + m_Matrix[2][1] *
2683  rPose2.GetY() + m_Matrix[2][2] * rPose2.GetHeading());
2684 
2685  return pose2;
2686  }
2687 
2692  inline void operator += (const Matrix3& rkMatrix)
2693  {
2694  for (kt_int32u row = 0; row < 3; row++)
2695  {
2696  for (kt_int32u col = 0; col < 3; col++)
2697  {
2698  m_Matrix[row][col] += rkMatrix.m_Matrix[row][col];
2699  }
2700  }
2701  }
2702 
2708  friend inline std::ostream& operator << (std::ostream& rStream, const Matrix3& rMatrix)
2709  {
2710  rStream << rMatrix.ToString();
2711  return rStream;
2712  }
2713 
2714  private:
2717  template<class Archive>
2718  void serialize(Archive &ar, const unsigned int version)
2719  {
2720  ar & BOOST_SERIALIZATION_NVP(m_Matrix);
2721  }
2722  }; // Matrix3
2723 
2727 
2731  class Matrix
2732  {
2733  public:
2737  Matrix(kt_int32u rows, kt_int32u columns)
2738  : m_Rows(rows)
2739  , m_Columns(columns)
2740  , m_pData(NULL)
2741  {
2742  Allocate();
2743 
2744  Clear();
2745  }
2746 
2750  virtual ~Matrix()
2751  {
2752  delete [] m_pData;
2753  }
2754 
2755  public:
2759  void Clear()
2760  {
2761  if (m_pData != NULL)
2762  {
2763  memset(m_pData, 0, sizeof(kt_double) * m_Rows * m_Columns);
2764  }
2765  }
2766 
2771  inline kt_int32u GetRows() const
2772  {
2773  return m_Rows;
2774  }
2775 
2780  inline kt_int32u GetColumns() const
2781  {
2782  return m_Columns;
2783  }
2784 
2792  {
2793  RangeCheck(row, column);
2794 
2795  return m_pData[row + column * m_Rows];
2796  }
2797 
2804  inline const kt_double& operator()(kt_int32u row, kt_int32u column) const
2805  {
2806  RangeCheck(row, column);
2807 
2808  return m_pData[row + column * m_Rows];
2809  }
2810 
2811  private:
2815  void Allocate()
2816  {
2817  try
2818  {
2819  if (m_pData != NULL)
2820  {
2821  delete[] m_pData;
2822  }
2823 
2824  m_pData = new kt_double[m_Rows * m_Columns];
2825  }
2826  catch (const std::bad_alloc& ex)
2827  {
2828  throw Exception("Matrix allocation error");
2829  }
2830 
2831  if (m_pData == NULL)
2832  {
2833  throw Exception("Matrix allocation error");
2834  }
2835  }
2836 
2842  inline void RangeCheck(kt_int32u row, kt_int32u column) const
2843  {
2844  if (math::IsUpTo(row, m_Rows) == false)
2845  {
2846  throw Exception("Matrix - RangeCheck ERROR!!!!");
2847  }
2848 
2849  if (math::IsUpTo(column, m_Columns) == false)
2850  {
2851  throw Exception("Matrix - RangeCheck ERROR!!!!");
2852  }
2853  }
2854 
2855  private:
2858 
2860  }; // Matrix
2861 
2865 
2870  {
2871  public:
2872  /*
2873  * Default constructor
2874  */
2876  : m_Minimum(999999999999999999.99999, 999999999999999999.99999)
2877  , m_Maximum(-999999999999999999.99999, -999999999999999999.99999)
2878  {
2879  }
2880 
2881  public:
2885  inline const Vector2<kt_double>& GetMinimum() const
2886  {
2887  return m_Minimum;
2888  }
2889 
2893  inline void SetMinimum(const Vector2<kt_double>& mMinimum)
2894  {
2895  m_Minimum = mMinimum;
2896  }
2897 
2901  inline const Vector2<kt_double>& GetMaximum() const
2902  {
2903  return m_Maximum;
2904  }
2905 
2909  inline void SetMaximum(const Vector2<kt_double>& rMaximum)
2910  {
2911  m_Maximum = rMaximum;
2912  }
2913 
2917  inline Size2<kt_double> GetSize() const
2918  {
2920 
2921  return Size2<kt_double>(size.GetX(), size.GetY());
2922  }
2923 
2927  inline void Add(const Vector2<kt_double>& rPoint)
2928  {
2929  m_Minimum.MakeFloor(rPoint);
2930  m_Maximum.MakeCeil(rPoint);
2931  }
2932 
2936  inline void Add(const BoundingBox2& rBoundingBox)
2937  {
2938  Add(rBoundingBox.GetMinimum());
2939  Add(rBoundingBox.GetMaximum());
2940  }
2941 
2947  inline kt_bool IsInBounds(const Vector2<kt_double>& rPoint) const
2948  {
2949  return (math::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) &&
2950  math::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY()));
2951  }
2952 
2957  template<class Archive>
2958  void serialize(Archive &ar, const unsigned int version)
2959  {
2960  ar & BOOST_SERIALIZATION_NVP(m_Minimum);
2961  ar & BOOST_SERIALIZATION_NVP(m_Maximum);
2962  }
2963 
2964  private:
2967  }; // BoundingBox2
2968 
2972 
2977  {
2978  public:
2983  Transform(const Pose2& rPose)
2984  {
2985  SetTransform(Pose2(), rPose);
2986  }
2987 
2993  Transform(const Pose2& rPose1, const Pose2& rPose2)
2994  {
2995  SetTransform(rPose1, rPose2);
2996  }
2997 
2998  public:
3004  inline Pose2 TransformPose(const Pose2& rSourcePose)
3005  {
3006  Pose2 newPosition = m_Transform + m_Rotation * rSourcePose;
3007  kt_double angle = math::NormalizeAngle(rSourcePose.GetHeading() + m_Transform.GetHeading());
3008 
3009  return Pose2(newPosition.GetPosition(), angle);
3010  }
3011 
3017  inline Pose2 InverseTransformPose(const Pose2& rSourcePose)
3018  {
3019  Pose2 newPosition = m_InverseRotation * (rSourcePose - m_Transform);
3021 
3022  // components of transform
3023  return Pose2(newPosition.GetPosition(), angle);
3024  }
3025 
3026  private:
3032  void SetTransform(const Pose2& rPose1, const Pose2& rPose2)
3033  {
3034  if (rPose1 == rPose2)
3035  {
3038  m_Transform = Pose2();
3039  return;
3040  }
3041 
3042  // heading transformation
3043  m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading());
3044  m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading());
3045 
3046  // position transformation
3047  Pose2 newPosition;
3048  if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0)
3049  {
3050  newPosition = rPose2 - m_Rotation * rPose1;
3051  }
3052  else
3053  {
3054  newPosition = rPose2;
3055  }
3056 
3057  m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading());
3058  }
3059 
3060  private:
3061  // pose transformation
3063 
3066 
3068  template<class Archive>
3069  void serialize(Archive &ar, const unsigned int version)
3070  {
3071  ar & BOOST_SERIALIZATION_NVP(m_Transform);
3072  ar & BOOST_SERIALIZATION_NVP(m_Rotation);
3073  ar & BOOST_SERIALIZATION_NVP(m_InverseRotation);
3074  }
3075  }; // Transform
3076 
3080 
3084  typedef enum
3085  {
3087 
3091 
3095 
3099 
3104  {
3105 
3106  public:
3108  {
3109  }
3115  AbstractParameter(const std::string& rName, ParameterManager* pParameterManger = NULL)
3116  : m_Name(rName)
3117  {
3118  // if parameter manager is provided add myself to it!
3119  if (pParameterManger != NULL)
3120  {
3121  pParameterManger->Add(this);
3122  }
3123  }
3124 
3131  AbstractParameter(const std::string& rName,
3132  const std::string& rDescription,
3133  ParameterManager* pParameterManger = NULL)
3134  : m_Name(rName)
3135  , m_Description(rDescription)
3136  {
3137  // if parameter manager is provided add myself to it!
3138  if (pParameterManger != NULL)
3139  {
3140  pParameterManger->Add(this);
3141  }
3142  }
3143 
3148  {
3149  }
3150 
3151  public:
3156  inline const std::string& GetName() const
3157  {
3158  return m_Name;
3159  }
3160 
3165  inline const std::string& GetDescription() const
3166  {
3167  return m_Description;
3168  }
3169 
3174  virtual const std::string GetValueAsString() const = 0;
3175 
3180  virtual void SetValueFromString(const std::string& rStringValue) = 0;
3181 
3186  virtual AbstractParameter* Clone() = 0;
3187 
3188  public:
3194  friend std::ostream& operator << (std::ostream& rStream, const AbstractParameter& rParameter)
3195  {
3196  rStream.precision(6);
3197  rStream.flags(std::ios::fixed);
3198 
3199  rStream << rParameter.GetName() << " = " << rParameter.GetValueAsString();
3200  return rStream;
3201  }
3202 
3203  private:
3204  std::string m_Name;
3205  std::string m_Description;
3210  template<class Archive>
3211  void serialize(Archive &ar, const unsigned int version)
3212  {
3213  ar & BOOST_SERIALIZATION_NVP(m_Name);
3214  ar & BOOST_SERIALIZATION_NVP(m_Description);
3215  }
3216  }; // AbstractParameter
3217  BOOST_SERIALIZATION_ASSUME_ABSTRACT(AbstractParameter)
3218 
3219 
3220 
3226  template<typename T>
3228  {
3229  public:
3237  {
3238  }
3239  Parameter(const std::string& rName, T value, ParameterManager* pParameterManger = NULL)
3240  : AbstractParameter(rName, pParameterManger)
3241  , m_Value(value)
3242  {
3243  }
3244 
3252  Parameter(const std::string& rName,
3253  const std::string& rDescription,
3254  T value,
3255  ParameterManager* pParameterManger = NULL)
3256  : AbstractParameter(rName, rDescription, pParameterManger)
3257  , m_Value(value)
3258  {
3259  }
3260 
3264  virtual ~Parameter()
3265  {
3266  }
3267 
3268  public:
3273  inline const T& GetValue() const
3274  {
3275  return m_Value;
3276  }
3277 
3282  inline void SetValue(const T& rValue)
3283  {
3284  m_Value = rValue;
3285  }
3286 
3291  virtual const std::string GetValueAsString() const
3292  {
3293  std::stringstream converter;
3294  converter << m_Value;
3295  return converter.str();
3296  }
3297 
3302  virtual void SetValueFromString(const std::string& rStringValue)
3303  {
3304  std::stringstream converter;
3305  converter.str(rStringValue);
3306  converter >> m_Value;
3307  }
3308 
3313  virtual Parameter* Clone()
3314  {
3315  return new Parameter(GetName(), GetDescription(), GetValue());
3316  }
3317 
3318  public:
3323  {
3324  m_Value = rOther.m_Value;
3325 
3326  return *this;
3327  }
3328 
3332  T operator = (T value)
3333  {
3334  m_Value = value;
3335 
3336  return m_Value;
3337  }
3338 
3342  friend class boost::serialization::access;
3343  template<class Archive>
3344  void serialize(Archive &ar, const unsigned int version)
3345  {
3346  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(AbstractParameter);
3347  ar & BOOST_SERIALIZATION_NVP(m_Value);
3348  }
3349 
3350  protected:
3355  }; // Parameter
3356  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Parameter)
3357 
3358  template<>
3359  inline void Parameter<kt_double>::SetValueFromString(const std::string& rStringValue)
3360  {
3361  int precision = std::numeric_limits<double>::digits10;
3362  std::stringstream converter;
3363  converter.precision(precision);
3364 
3365  converter.str(rStringValue);
3366 
3367  m_Value = 0.0;
3368  converter >> m_Value;
3369  }
3370 
3371  template<>
3372  inline const std::string Parameter<kt_double>::GetValueAsString() const
3373  {
3374  std::stringstream converter;
3375  converter.precision(std::numeric_limits<double>::digits10);
3376  converter << m_Value;
3377  return converter.str();
3378  }
3379 
3380  template<>
3381  inline void Parameter<kt_bool>::SetValueFromString(const std::string& rStringValue)
3382  {
3383  if (rStringValue == "true" || rStringValue == "TRUE")
3384  {
3385  m_Value = true;
3386  }
3387  else
3388  {
3389  m_Value = false;
3390  }
3391  }
3392 
3393  template<>
3394  inline const std::string Parameter<kt_bool>::GetValueAsString() const
3395  {
3396  if (m_Value == true)
3397  {
3398  return "true";
3399  }
3400 
3401  return "false";
3402  }
3403 
3407 
3411  class ParameterEnum : public Parameter<kt_int32s>
3412  {
3413  typedef std::map<std::string, kt_int32s> EnumMap;
3414 
3415  public:
3422  ParameterEnum(const std::string& rName, kt_int32s value, ParameterManager* pParameterManger = NULL)
3423  : Parameter<kt_int32s>(rName, value, pParameterManger)
3424  {
3425  }
3427  {
3428  }
3429 
3433  virtual ~ParameterEnum()
3434  {
3435  }
3436 
3437  public:
3443  {
3444  ParameterEnum* pEnum = new ParameterEnum(GetName(), GetValue());
3445 
3446  pEnum->m_EnumDefines = m_EnumDefines;
3447 
3448  return pEnum;
3449  }
3450 
3455  virtual void SetValueFromString(const std::string& rStringValue)
3456  {
3457  if (m_EnumDefines.find(rStringValue) != m_EnumDefines.end())
3458  {
3459  m_Value = m_EnumDefines[rStringValue];
3460  }
3461  else
3462  {
3463  std::string validValues;
3464 
3465  const_forEach(EnumMap, &m_EnumDefines)
3466  {
3467  validValues += iter->first + ", ";
3468  }
3469 
3470  throw Exception("Unable to set enum: " + rStringValue + ". Valid values are: " + validValues);
3471  }
3472  }
3473 
3478  virtual const std::string GetValueAsString() const
3479  {
3480  const_forEach(EnumMap, &m_EnumDefines)
3481  {
3482  if (iter->second == m_Value)
3483  {
3484  return iter->first;
3485  }
3486  }
3487 
3488  throw Exception("Unable to lookup enum");
3489  }
3490 
3496  void DefineEnumValue(kt_int32s value, const std::string& rName)
3497  {
3498  if (m_EnumDefines.find(rName) == m_EnumDefines.end())
3499  {
3500  m_EnumDefines[rName] = value;
3501  }
3502  else
3503  {
3504  std::cerr << "Overriding enum value: " << m_EnumDefines[rName] << " with " << value << std::endl;
3505 
3506  m_EnumDefines[rName] = value;
3507 
3508  assert(false);
3509  }
3510  }
3511 
3512  public:
3516  ParameterEnum& operator = (const ParameterEnum& rOther)
3517  {
3518  SetValue(rOther.GetValue());
3519 
3520  return *this;
3521  }
3522 
3526  kt_int32s operator = (kt_int32s value)
3527  {
3528  SetValue(value);
3529 
3530  return m_Value;
3531  }
3532 
3533  private:
3535 
3536  friend class boost::serialization::access;
3537  template<class Archive>
3538  void serialize(Archive &ar, const unsigned int version)
3539  {
3540  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Parameter<kt_int32s>);
3541  ar & BOOST_SERIALIZATION_NVP(m_EnumDefines);
3542  }
3543  }; // ParameterEnum
3544  BOOST_SERIALIZATION_ASSUME_ABSTRACT(ParameterEnum)
3548 
3552  class Parameters : public Object
3553  {
3554  public:
3555  // @cond EXCLUDE
3557  // @endcond
3558 
3559  public:
3565  {
3566  }
3567  Parameters(const std::string& rName)
3568  : Object(rName)
3569  {
3570  }
3571 
3575  virtual ~Parameters()
3576  {
3577  }
3578 
3579  friend class boost::serialization::access;
3580  template<class Archive>
3581  void serialize(Archive &ar, const unsigned int version)
3582  {
3583  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
3584  }
3585 
3586  private:
3587  Parameters(const Parameters&);
3588  const Parameters& operator=(const Parameters&);
3589  }; // Parameters
3590 
3594 
3595  class SensorData;
3596 
3600  class KARTO_EXPORT Sensor : public Object
3601  {
3602 
3606  public:
3608  {
3609  }
3610  // @cond EXCLUDE
3612  // @endcond
3613 
3614  protected:
3619  Sensor(const Name& rName);
3620 
3621  public:
3625  virtual ~Sensor();
3626 
3627  public:
3632  inline const Pose2& GetOffsetPose() const
3633  {
3634  return m_pOffsetPose->GetValue();
3635  }
3636 
3641  inline void SetOffsetPose(const Pose2& rPose)
3642  {
3643  m_pOffsetPose->SetValue(rPose);
3644  }
3645 
3650  virtual kt_bool Validate() = 0;
3651 
3657  virtual kt_bool Validate(SensorData* pSensorData) = 0;
3658 
3659  private:
3663  Sensor(const Sensor&);
3664 
3668  const Sensor& operator=(const Sensor&);
3669 
3670  private:
3675  friend class boost::serialization::access;
3676  template<class Archive>
3677  void serialize(Archive &ar, const unsigned int version)
3678  {
3679  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
3680  ar & BOOST_SERIALIZATION_NVP(m_pOffsetPose);
3681  }
3682  }; // Sensor
3683  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Sensor)
3687  typedef std::vector<Sensor*> SensorVector;
3688 
3692 
3696  typedef std::map<Name, Sensor*> SensorManagerMap;
3697 
3702  {
3703  public:
3708  {
3709  }
3710 
3714  virtual ~SensorManager()
3715  {
3716  }
3717 
3718  public:
3722  static SensorManager* GetInstance();
3723 
3724  public:
3732  void RegisterSensor(Sensor* pSensor, kt_bool override = false)
3733  {
3734  Validate(pSensor);
3735 
3736  if ((m_Sensors.find(pSensor->GetName()) != m_Sensors.end()) && !override)
3737  {
3738  throw Exception("Cannot register sensor: already registered: [" +
3739  pSensor->GetName().ToString() +
3740  "] (Consider setting 'override' to true)");
3741  }
3742 
3743  std::cout << "Registering sensor: [" << pSensor->GetName().ToString() << "]" << std::endl;
3744 
3745  m_Sensors[pSensor->GetName()] = pSensor;
3746  }
3747 
3752  void UnregisterSensor(Sensor* pSensor)
3753  {
3754  Validate(pSensor);
3755 
3756  if (m_Sensors.find(pSensor->GetName()) != m_Sensors.end())
3757  {
3758  std::cout << "Unregistering sensor: " << pSensor->GetName().ToString() << std::endl;
3759 
3760  m_Sensors.erase(pSensor->GetName());
3761  }
3762  else
3763  {
3764  throw Exception("Cannot unregister sensor: not registered: [" + pSensor->GetName().ToString() + "]");
3765  }
3766  }
3767 
3773  Sensor* GetSensorByName(const Name& rName)
3774  {
3775  if (m_Sensors.find(rName) != m_Sensors.end())
3776  {
3777  return m_Sensors[rName];
3778  }
3779 
3780  throw Exception("Sensor not registered: [" + rName.ToString() + "] (Did you add the sensor to the Dataset?)");
3781  }
3782 
3788  template<class T>
3789  T* GetSensorByName(const Name& rName)
3790  {
3791  Sensor* pSensor = GetSensorByName(rName);
3792 
3793  return dynamic_cast<T*>(pSensor);
3794  }
3795 
3801  {
3802  SensorVector sensors;
3803 
3804  forEach(SensorManagerMap, &m_Sensors)
3805  {
3806  sensors.push_back(iter->second);
3807  }
3808 
3809  return sensors;
3810  }
3811 
3812  protected:
3817  static void Validate(Sensor* pSensor)
3818  {
3819  if (pSensor == NULL)
3820  {
3821  throw Exception("Invalid sensor: NULL");
3822  }
3823  else if (pSensor->GetName().ToString() == "")
3824  {
3825  throw Exception("Invalid sensor: nameless");
3826  }
3827  }
3828 
3829  protected:
3834 
3835  friend class boost::serialization::access;
3836  template<class Archive>
3837  void serialize(Archive &ar, const unsigned int version)
3838  {
3839  ar & BOOST_SERIALIZATION_NVP(m_Sensors);
3840  }
3841  };
3842 
3846 
3853  class Drive : public Sensor
3854  {
3855  public:
3856  // @cond EXCLUDE
3858  // @endcond
3859 
3860  public:
3864  Drive(const std::string& rName)
3865  : Sensor(rName)
3866  {
3867  }
3871  virtual ~Drive()
3872  {
3873  }
3874 
3875  public:
3876  virtual kt_bool Validate()
3877  {
3878  return true;
3879  }
3880 
3881  virtual kt_bool Validate(SensorData* pSensorData)
3882  {
3883  if (pSensorData == NULL)
3884  {
3885  return false;
3886  }
3887 
3888  return true;
3889  }
3890 
3891  private:
3892  Drive(const Drive&);
3893  const Drive& operator=(const Drive&);
3894  friend class boost::serialization::access;
3895  template<class Archive>
3896  void serialize(Archive &ar, const unsigned int version)
3897  {
3898  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Sensor);
3899  }
3900  }; // class Drive
3901 
3902  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Drive)
3906 
3907  class LocalizedRangeScan;
3908  class CoordinateConverter;
3909 
3923  {
3924  public:
3926  {
3927  }
3928  // @cond EXCLUDE
3930  // @endcond
3931 
3932  public:
3937  {
3938  }
3939 
3940  public:
3946  {
3947  return m_pMinimumRange->GetValue();
3948  }
3949 
3954  inline void SetMinimumRange(kt_double minimumRange)
3955  {
3956  m_pMinimumRange->SetValue(minimumRange);
3957 
3958  SetRangeThreshold(GetRangeThreshold());
3959  }
3960 
3966  {
3967  return m_pMaximumRange->GetValue();
3968  }
3969 
3974  inline void SetMaximumRange(kt_double maximumRange)
3975  {
3976  m_pMaximumRange->SetValue(maximumRange);
3977 
3978  SetRangeThreshold(GetRangeThreshold());
3979  }
3980 
3986  {
3987  return m_pRangeThreshold->GetValue();
3988  }
3989 
3994  inline void SetRangeThreshold(kt_double rangeThreshold)
3995  {
3996  // make sure rangeThreshold is within laser range finder range
3997  m_pRangeThreshold->SetValue(math::Clip(rangeThreshold, GetMinimumRange(), GetMaximumRange()));
3998 
3999  if (math::DoubleEqual(GetRangeThreshold(), rangeThreshold) == false)
4000  {
4001  std::cout << "Info: clipped range threshold to be within minimum and maximum range!" << std::endl;
4002  }
4003  }
4004 
4010  {
4011  return m_pMinimumAngle->GetValue();
4012  }
4013 
4018  inline void SetMinimumAngle(kt_double minimumAngle)
4019  {
4020  m_pMinimumAngle->SetValue(minimumAngle);
4021 
4022  Update();
4023  }
4024 
4030  {
4031  return m_pMaximumAngle->GetValue();
4032  }
4033 
4038  inline void SetMaximumAngle(kt_double maximumAngle)
4039  {
4040  m_pMaximumAngle->SetValue(maximumAngle);
4041 
4042  Update();
4043  }
4044 
4050  {
4051  return m_pAngularResolution->GetValue();
4052  }
4053 
4058  inline void SetAngularResolution(kt_double angularResolution)
4059  {
4060  if (m_pType->GetValue() == LaserRangeFinder_Custom)
4061  {
4062  m_pAngularResolution->SetValue(angularResolution);
4063  }
4064  else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS100)
4065  {
4066  if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
4067  {
4068  m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
4069  }
4070  else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
4071  {
4072  m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
4073  }
4074  else
4075  {
4076  std::stringstream stream;
4077  stream << "Invalid resolution for Sick LMS100: ";
4078  stream << angularResolution;
4079  throw Exception(stream.str());
4080  }
4081  }
4082  else if (m_pType->GetValue() == LaserRangeFinder_Sick_LMS200 ||
4083  m_pType->GetValue() == LaserRangeFinder_Sick_LMS291)
4084  {
4085  if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.25)))
4086  {
4087  m_pAngularResolution->SetValue(math::DegreesToRadians(0.25));
4088  }
4089  else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(0.50)))
4090  {
4091  m_pAngularResolution->SetValue(math::DegreesToRadians(0.50));
4092  }
4093  else if (math::DoubleEqual(angularResolution, math::DegreesToRadians(1.00)))
4094  {
4095  m_pAngularResolution->SetValue(math::DegreesToRadians(1.00));
4096  }
4097  else
4098  {
4099  std::stringstream stream;
4100  stream << "Invalid resolution for Sick LMS291: ";
4101  stream << angularResolution;
4102  throw Exception(stream.str());
4103  }
4104  }
4105  else
4106  {
4107  throw Exception("Can't set angular resolution, please create a LaserRangeFinder of type Custom");
4108  }
4109 
4110  Update();
4111  }
4112 
4117  {
4118  return m_pType->GetValue();
4119  }
4120 
4126  {
4127  return m_NumberOfRangeReadings;
4128  }
4129 
4130 
4135  inline kt_bool GetIs360Laser() const
4136  {
4137  return m_pIs360Laser->GetValue();
4138  }
4139 
4144  inline void SetIs360Laser(bool is_360_laser)
4145  {
4146  m_pIs360Laser->SetValue(is_360_laser);
4147 
4148  Update();
4149  }
4150 
4151 
4152  virtual kt_bool Validate()
4153  {
4154  Update();
4155 
4156  if (math::InRange(GetRangeThreshold(), GetMinimumRange(), GetMaximumRange()) == false)
4157  {
4158  std::cout << "Please set range threshold to a value between ["
4159  << GetMinimumRange() << ";" << GetMaximumRange() << "]" << std::endl;
4160  return false;
4161  }
4162 
4163  return true;
4164  }
4165 
4166  virtual kt_bool Validate(SensorData* pSensorData);
4167 
4175  const PointVectorDouble GetPointReadings(LocalizedRangeScan* pLocalizedRangeScan,
4176  CoordinateConverter* pCoordinateConverter,
4177  kt_bool ignoreThresholdPoints = true,
4178  kt_bool flipY = false) const;
4179 
4180  public:
4188  {
4189  LaserRangeFinder* pLrf = NULL;
4190 
4191  switch (type)
4192  {
4193  // see http://www.hizook.com/files/publications/SICK_LMS100.pdf
4194  // set range threshold to 18m
4196  {
4197  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 100"));
4198 
4199  // Sensing range is 18 meters (at 10% reflectivity, max range of 20 meters), with an error of about 20mm
4200  pLrf->m_pMinimumRange->SetValue(0.0);
4201  pLrf->m_pMaximumRange->SetValue(20.0);
4202 
4203  // 270 degree range, 50 Hz
4206 
4207  // 0.25 degree angular resolution
4209 
4210  pLrf->m_NumberOfRangeReadings = 1081;
4211  }
4212  break;
4213 
4214  // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
4215  // set range threshold to 10m
4217  {
4218  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 200"));
4219 
4220  // Sensing range is 80 meters
4221  pLrf->m_pMinimumRange->SetValue(0.0);
4222  pLrf->m_pMaximumRange->SetValue(80.0);
4223 
4224  // 180 degree range, 75 Hz
4227 
4228  // 0.5 degree angular resolution
4230 
4231  pLrf->m_NumberOfRangeReadings = 361;
4232  }
4233  break;
4234 
4235  // see http://www.hizook.com/files/publications/SICK_LMS200-291_Tech_Info.pdf
4236  // set range threshold to 30m
4238  {
4239  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Sick LMS 291"));
4240 
4241  // Sensing range is 80 meters
4242  pLrf->m_pMinimumRange->SetValue(0.0);
4243  pLrf->m_pMaximumRange->SetValue(80.0);
4244 
4245  // 180 degree range, 75 Hz
4248 
4249  // 0.5 degree angular resolution
4251 
4252  pLrf->m_NumberOfRangeReadings = 361;
4253  }
4254  break;
4255 
4256  // see http://www.hizook.com/files/publications/Hokuyo_UTM_LaserRangeFinder_LIDAR.pdf
4257  // set range threshold to 30m
4259  {
4260  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo UTM-30LX"));
4261 
4262  // Sensing range is 30 meters
4263  pLrf->m_pMinimumRange->SetValue(0.1);
4264  pLrf->m_pMaximumRange->SetValue(30.0);
4265 
4266  // 270 degree range, 40 Hz
4269 
4270  // 0.25 degree angular resolution
4272 
4273  pLrf->m_NumberOfRangeReadings = 1081;
4274  }
4275  break;
4276 
4277  // see http://www.hizook.com/files/publications/HokuyoURG_Datasheet.pdf
4278  // set range threshold to 3.5m
4280  {
4281  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("Hokuyo URG-04LX"));
4282 
4283  // Sensing range is 4 meters. It has detection problems when
4284  // scanning absorptive surfaces (such as black trimming).
4285  pLrf->m_pMinimumRange->SetValue(0.02);
4286  pLrf->m_pMaximumRange->SetValue(4.0);
4287 
4288  // 240 degree range, 10 Hz
4291 
4292  // 0.352 degree angular resolution
4294 
4295  pLrf->m_NumberOfRangeReadings = 751;
4296  }
4297  break;
4298 
4300  {
4301  pLrf = new LaserRangeFinder((rName.GetName() != "") ? rName : Name("User-Defined LaserRangeFinder"));
4302 
4303  // Sensing range is 80 meters.
4304  pLrf->m_pMinimumRange->SetValue(0.0);
4305  pLrf->m_pMaximumRange->SetValue(80.0);
4306 
4307  // 180 degree range
4310 
4311  // 1.0 degree angular resolution
4313 
4314  pLrf->m_NumberOfRangeReadings = 181;
4315  }
4316  break;
4317  }
4318 
4319  if (pLrf != NULL)
4320  {
4321  pLrf->m_pType->SetValue(type);
4322 
4323  Pose2 defaultOffset;
4324  pLrf->SetOffsetPose(defaultOffset);
4325  }
4326 
4327  return pLrf;
4328  }
4329 
4330  private:
4334  LaserRangeFinder(const Name& rName)
4335  : Sensor(rName)
4336  , m_NumberOfRangeReadings(0)
4337  {
4338  m_pMinimumRange = new Parameter<kt_double>("MinimumRange", 0.0, GetParameterManager());
4339  m_pMaximumRange = new Parameter<kt_double>("MaximumRange", 80.0, GetParameterManager());
4340 
4341  m_pMinimumAngle = new Parameter<kt_double>("MinimumAngle", -KT_PI_2, GetParameterManager());
4342  m_pMaximumAngle = new Parameter<kt_double>("MaximumAngle", KT_PI_2, GetParameterManager());
4343 
4344  m_pAngularResolution = new Parameter<kt_double>("AngularResolution",
4347 
4348  m_pRangeThreshold = new Parameter<kt_double>("RangeThreshold", 12.0, GetParameterManager());
4349 
4350  m_pIs360Laser = new Parameter<kt_bool>("Is360DegreeLaser", false, GetParameterManager());
4351 
4352  m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager());
4353  m_pType->DefineEnumValue(LaserRangeFinder_Custom, "Custom");
4354  m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS100, "Sick_LMS100");
4355  m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS200, "Sick_LMS200");
4356  m_pType->DefineEnumValue(LaserRangeFinder_Sick_LMS291, "Sick_LMS291");
4357  m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_UTM_30LX, "Hokuyo_UTM_30LX");
4358  m_pType->DefineEnumValue(LaserRangeFinder_Hokuyo_URG_04LX, "Hokuyo_URG_04LX");
4359  }
4360 
4365  void Update()
4366  {
4367  int residual = 1;
4368  if (GetIs360Laser())
4369  {
4370  // residual is 0 by 360 lidar conventions
4371  residual = 0;
4372  }
4373  m_NumberOfRangeReadings = static_cast<kt_int32u>(math::Round((GetMaximumAngle() -
4374  GetMinimumAngle())
4375  / GetAngularResolution()) + residual);
4376  }
4377 
4378  private:
4381 
4382  private:
4383  // sensor m_Parameters
4386 
4388 
4391 
4393 
4395 
4397 
4399 
4400  // static std::string LaserRangeFinderTypeNames[6];
4401  friend class boost::serialization::access;
4402  template<class Archive>
4403  void serialize(Archive &ar, const unsigned int version)
4404  {
4405  if (Archive::is_loading::value)
4406  {
4407  m_pMinimumRange = new Parameter<kt_double>("MinimumRange", 0.0, GetParameterManager());
4408  m_pMaximumRange = new Parameter<kt_double>("MaximumRange", 80.0, GetParameterManager());
4409 
4410  m_pMinimumAngle = new Parameter<kt_double>("MinimumAngle", -KT_PI_2, GetParameterManager());
4411  m_pMaximumAngle = new Parameter<kt_double>("MaximumAngle", KT_PI_2, GetParameterManager());
4412 
4413  m_pAngularResolution = new Parameter<kt_double>("AngularResolution",
4416 
4417  m_pRangeThreshold = new Parameter<kt_double>("RangeThreshold", 12.0, GetParameterManager());
4418 
4419  m_pIs360Laser = new Parameter<kt_bool>("Is360DegreeLaser", false, GetParameterManager());
4420 
4421  m_pType = new ParameterEnum("Type", LaserRangeFinder_Custom, GetParameterManager());
4422  }
4423 
4424  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Sensor);
4425  ar & BOOST_SERIALIZATION_NVP(m_pMinimumAngle);
4426  ar & BOOST_SERIALIZATION_NVP(m_pMaximumAngle);
4427  ar & BOOST_SERIALIZATION_NVP(m_pAngularResolution);
4428  ar & BOOST_SERIALIZATION_NVP(m_pMinimumRange);
4429  ar & BOOST_SERIALIZATION_NVP(m_pMaximumRange);
4430  ar & BOOST_SERIALIZATION_NVP(m_pRangeThreshold);
4431  ar & BOOST_SERIALIZATION_NVP(m_pIs360Laser);
4432  ar & BOOST_SERIALIZATION_NVP(m_pType);
4433  ar & BOOST_SERIALIZATION_NVP(m_NumberOfRangeReadings);
4434  }
4435  }; // LaserRangeFinder
4436  BOOST_SERIALIZATION_ASSUME_ABSTRACT(LaserRangeFinder)
4440 
4444  typedef enum
4445  {
4449  } GridStates;
4450 
4454 
4461  {
4462  public:
4467  : m_Scale(20.0)
4468  {
4469  }
4470 
4471  public:
4478  {
4479  return value * m_Scale;
4480  }
4481 
4488  inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double>& rWorld, kt_bool flipY = false) const
4489  {
4490  kt_double gridX = (rWorld.GetX() - m_Offset.GetX()) * m_Scale;
4491  kt_double gridY = 0.0;
4492 
4493  if (flipY == false)
4494  {
4495  gridY = (rWorld.GetY() - m_Offset.GetY()) * m_Scale;
4496  }
4497  else
4498  {
4499  gridY = (m_Size.GetHeight() / m_Scale - rWorld.GetY() + m_Offset.GetY()) * m_Scale;
4500  }
4501 
4502  return Vector2<kt_int32s>(static_cast<kt_int32s>(math::Round(gridX)), static_cast<kt_int32s>(math::Round(gridY)));
4503  }
4504 
4511  inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s>& rGrid, kt_bool flipY = false) const
4512  {
4513  kt_double worldX = m_Offset.GetX() + rGrid.GetX() / m_Scale;
4514  kt_double worldY = 0.0;
4515 
4516  if (flipY == false)
4517  {
4518  worldY = m_Offset.GetY() + rGrid.GetY() / m_Scale;
4519  }
4520  else
4521  {
4522  worldY = m_Offset.GetY() + (m_Size.GetHeight() - rGrid.GetY()) / m_Scale;
4523  }
4524 
4525  return Vector2<kt_double>(worldX, worldY);
4526  }
4527 
4532  inline kt_double GetScale() const
4533  {
4534  return m_Scale;
4535  }
4536 
4541  inline void SetScale(kt_double scale)
4542  {
4543  m_Scale = scale;
4544  }
4545 
4550  inline const Vector2<kt_double>& GetOffset() const
4551  {
4552  return m_Offset;
4553  }
4554 
4559  inline void SetOffset(const Vector2<kt_double>& rOffset)
4560  {
4561  m_Offset = rOffset;
4562  }
4563 
4568  inline void SetSize(const Size2<kt_int32s>& rSize)
4569  {
4570  m_Size = rSize;
4571  }
4572 
4577  inline const Size2<kt_int32s>& GetSize() const
4578  {
4579  return m_Size;
4580  }
4581 
4586  inline kt_double GetResolution() const
4587  {
4588  return 1.0 / m_Scale;
4589  }
4590 
4595  inline void SetResolution(kt_double resolution)
4596  {
4597  m_Scale = 1.0 / resolution;
4598  }
4599 
4605  {
4606  BoundingBox2 box;
4607 
4608  kt_double minX = GetOffset().GetX();
4609  kt_double minY = GetOffset().GetY();
4610  kt_double maxX = minX + GetSize().GetWidth() * GetResolution();
4611  kt_double maxY = minY + GetSize().GetHeight() * GetResolution();
4612 
4613  box.SetMinimum(GetOffset());
4614  box.SetMaximum(Vector2<kt_double>(maxX, maxY));
4615  return box;
4616  }
4617 
4618  private:
4621 
4623  friend class boost::serialization::access;
4624  template<class Archive>
4625  void serialize(Archive &ar, const unsigned int version)
4626  {
4627  ar & BOOST_SERIALIZATION_NVP(m_Size);
4628  ar & BOOST_SERIALIZATION_NVP(m_Scale);
4629  ar & BOOST_SERIALIZATION_NVP(m_Offset);
4630  }
4631 
4632  }; // CoordinateConverter
4633 
4637 
4641  template<typename T>
4642  class Grid
4643  {
4644  public:
4653  {
4654  }
4655  static Grid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
4656  {
4657  Grid* pGrid = new Grid(width, height);
4658 
4659  pGrid->GetCoordinateConverter()->SetScale(1.0 / resolution);
4660 
4661  return pGrid;
4662  }
4663 
4667  virtual ~Grid()
4668  {
4669  if (m_pData)
4670  {
4671  delete [] m_pData;
4672  }
4673  if (m_pCoordinateConverter)
4674  {
4675  delete m_pCoordinateConverter;
4676  }
4677  }
4678 
4679  public:
4683  void Clear()
4684  {
4685  memset(m_pData, 0, GetDataSize() * sizeof(T));
4686  }
4687 
4693  {
4694  Grid* pGrid = CreateGrid(GetWidth(), GetHeight(), GetResolution());
4695  pGrid->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
4696 
4697  memcpy(pGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
4698 
4699  return pGrid;
4700  }
4701 
4707  virtual void Resize(kt_int32s width, kt_int32s height)
4708  {
4709  m_Width = width;
4710  m_Height = height;
4711  m_WidthStep = math::AlignValue<kt_int32s>(width, 8);
4712 
4713  if (m_pData != NULL)
4714  {
4715  delete[] m_pData;
4716  m_pData = NULL;
4717  }
4718 
4719  try
4720  {
4721  m_pData = new T[GetDataSize()];
4722 
4723  if (m_pCoordinateConverter == NULL)
4724  {
4725  m_pCoordinateConverter = new CoordinateConverter();
4726  }
4727 
4728  m_pCoordinateConverter->SetSize(Size2<kt_int32s>(width, height));
4729  }
4730  catch(...)
4731  {
4732  m_pData = NULL;
4733 
4734  m_Width = 0;
4735  m_Height = 0;
4736  m_WidthStep = 0;
4737  }
4738 
4739  Clear();
4740  }
4741 
4746  inline kt_bool IsValidGridIndex(const Vector2<kt_int32s>& rGrid) const
4747  {
4748  return (math::IsUpTo(rGrid.GetX(), m_Width) && math::IsUpTo(rGrid.GetY(), m_Height));
4749  }
4750 
4757  virtual kt_int32s GridIndex(const Vector2<kt_int32s>& rGrid, kt_bool boundaryCheck = true) const
4758  {
4759  if (boundaryCheck == true)
4760  {
4761  if (IsValidGridIndex(rGrid) == false)
4762  {
4763  std::stringstream error;
4764  error << "Index " << rGrid << " out of range. Index must be between [0; "
4765  << m_Width << ") and [0; " << m_Height << ")";
4766  throw Exception(error.str());
4767  }
4768  }
4769 
4770  kt_int32s index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep);
4771 
4772  if (boundaryCheck == true)
4773  {
4774  assert(math::IsUpTo(index, GetDataSize()));
4775  }
4776 
4777  return index;
4778  }
4779 
4786  {
4787  Vector2<kt_int32s> grid;
4788 
4789  grid.SetY(index / m_WidthStep);
4790  grid.SetX(index - grid.GetY() * m_WidthStep);
4791 
4792  return grid;
4793  }
4794 
4801  inline Vector2<kt_int32s> WorldToGrid(const Vector2<kt_double>& rWorld, kt_bool flipY = false) const
4802  {
4803  return GetCoordinateConverter()->WorldToGrid(rWorld, flipY);
4804  }
4805 
4812  inline Vector2<kt_double> GridToWorld(const Vector2<kt_int32s>& rGrid, kt_bool flipY = false) const
4813  {
4814  return GetCoordinateConverter()->GridToWorld(rGrid, flipY);
4815  }
4816 
4823  {
4824  kt_int32s index = GridIndex(rGrid, true);
4825  return m_pData + index;
4826  }
4827 
4833  T* GetDataPointer(const Vector2<kt_int32s>& rGrid) const
4834  {
4835  kt_int32s index = GridIndex(rGrid, true);
4836  return m_pData + index;
4837  }
4838 
4843  inline kt_int32s GetWidth() const
4844  {
4845  return m_Width;
4846  };
4847 
4852  inline kt_int32s GetHeight() const
4853  {
4854  return m_Height;
4855  };
4856 
4861  inline const Size2<kt_int32s> GetSize() const
4862  {
4863  return Size2<kt_int32s>(m_Width, m_Height);
4864  }
4865 
4870  inline kt_int32s GetWidthStep() const
4871  {
4872  return m_WidthStep;
4873  }
4874 
4879  inline T* GetDataPointer()
4880  {
4881  return m_pData;
4882  }
4883 
4888  inline T* GetDataPointer() const
4889  {
4890  return m_pData;
4891  }
4892 
4897  inline kt_int32s GetDataSize() const
4898  {
4899  return m_WidthStep * m_Height;
4900  }
4901 
4907  inline T GetValue(const Vector2<kt_int32s>& rGrid) const
4908  {
4909  kt_int32s index = GridIndex(rGrid);
4910  return m_pData[index];
4911  }
4912 
4918  {
4919  return m_pCoordinateConverter;
4920  }
4921 
4926  inline kt_double GetResolution() const
4927  {
4928  return GetCoordinateConverter()->GetResolution();
4929  }
4930 
4936  {
4937  return GetCoordinateConverter()->GetBoundingBox();
4938  }
4939 
4949  void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor* f = NULL)
4950  {
4951  kt_bool steep = abs(y1 - y0) > abs(x1 - x0);
4952  if (steep)
4953  {
4954  std::swap(x0, y0);
4955  std::swap(x1, y1);
4956  }
4957  if (x0 > x1)
4958  {
4959  std::swap(x0, x1);
4960  std::swap(y0, y1);
4961  }
4962 
4963  kt_int32s deltaX = x1 - x0;
4964  kt_int32s deltaY = abs(y1 - y0);
4965  kt_int32s error = 0;
4966  kt_int32s ystep;
4967  kt_int32s y = y0;
4968 
4969  if (y0 < y1)
4970  {
4971  ystep = 1;
4972  }
4973  else
4974  {
4975  ystep = -1;
4976  }
4977 
4978  kt_int32s pointX;
4979  kt_int32s pointY;
4980  for (kt_int32s x = x0; x <= x1; x++)
4981  {
4982  if (steep)
4983  {
4984  pointX = y;
4985  pointY = x;
4986  }
4987  else
4988  {
4989  pointX = x;
4990  pointY = y;
4991  }
4992 
4993  error += deltaY;
4994 
4995  if (2 * error >= deltaX)
4996  {
4997  y += ystep;
4998  error -= deltaX;
4999  }
5000 
5001  Vector2<kt_int32s> gridIndex(pointX, pointY);
5002  if (IsValidGridIndex(gridIndex))
5003  {
5004  kt_int32s index = GridIndex(gridIndex, false);
5005  T* pGridPointer = GetDataPointer();
5006  pGridPointer[index]++;
5007 
5008  if (f != NULL)
5009  {
5010  (*f)(index);
5011  }
5012  }
5013  }
5014  }
5015 
5016  protected:
5022  Grid(kt_int32s width, kt_int32s height)
5023  : m_pData(NULL)
5024  , m_pCoordinateConverter(NULL)
5025  {
5026  Resize(width, height);
5027  }
5028 
5029  private:
5030  kt_int32s m_Width; // width of grid
5031  kt_int32s m_Height; // height of grid
5032  kt_int32s m_WidthStep; // 8 bit aligned width of grid
5033  T* m_pData; // grid data
5034 
5035  // coordinate converter to convert between world coordinates and grid coordinates
5037  friend class boost::serialization::access;
5038  template<class Archive>
5039  void serialize(Archive &ar, const unsigned int version)
5040  {
5041  ar & BOOST_SERIALIZATION_NVP(m_Width);
5042  ar & BOOST_SERIALIZATION_NVP(m_Height);
5043  ar & BOOST_SERIALIZATION_NVP(m_WidthStep);
5044  ar & BOOST_SERIALIZATION_NVP(m_pCoordinateConverter);
5045 
5046 
5047  if (Archive::is_loading::value)
5048  {
5049  m_pData = new T[m_WidthStep * m_Height];
5050  }
5051  ar & boost::serialization::make_array<T>(m_pData, m_WidthStep * m_Height);
5052  }
5053 
5054  }; // Grid
5055  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Grid)
5056 
5057 
5058 
5064  class CustomData : public Object
5065  {
5066  public:
5067  // @cond EXCLUDE
5069  // @endcond
5070 
5071  public:
5076  : Object()
5077  {
5078  }
5079 
5083  virtual ~CustomData()
5084  {
5085  }
5086 
5087  public:
5092  virtual const std::string Write() const = 0;
5093 
5098  virtual void Read(const std::string& rValue) = 0;
5099 
5100  private:
5101  CustomData(const CustomData&);
5102  const CustomData& operator=(const CustomData&);
5103 
5104  friend class boost::serialization::access;
5105  template<class Archive>
5106  void serialize(Archive &ar, const unsigned int version)
5107  {
5108  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
5109  }
5110  };
5111  BOOST_SERIALIZATION_ASSUME_ABSTRACT(CustomData)
5112 
5113 
5116  typedef std::vector<CustomData*> CustomDataVector;
5117 
5121 
5126  {
5127  public:
5128  // @cond EXCLUDE
5130  // @endcond
5131 
5132  public:
5133 
5138  virtual ~SensorData();
5139 
5140  public:
5145  inline kt_int32s GetStateId() const
5146  {
5147  return m_StateId;
5148  }
5149 
5154  inline void SetStateId(kt_int32s stateId)
5155  {
5156  m_StateId = stateId;
5157  }
5158 
5163  inline kt_int32s GetUniqueId() const
5164  {
5165  return m_UniqueId;
5166  }
5167 
5172  inline void SetUniqueId(kt_int32u uniqueId)
5173  {
5174  m_UniqueId = uniqueId;
5175  }
5176 
5181  inline kt_double GetTime() const
5182  {
5183  return m_Time;
5184  }
5185 
5190  inline void SetTime(kt_double time)
5191  {
5192  m_Time = time;
5193  }
5194 
5199  inline const Name& GetSensorName() const
5200  {
5201  return m_SensorName;
5202  }
5203 
5208  inline void AddCustomData(CustomData* pCustomData)
5209  {
5210  m_CustomData.push_back(pCustomData);
5211  }
5212 
5217  inline const CustomDataVector& GetCustomData() const
5218  {
5219  return m_CustomData;
5220  }
5221 
5222  protected:
5226  SensorData(const Name& rSensorName);
5227 
5228  private:
5232  SensorData(const SensorData&);
5233 
5237  const SensorData& operator=(const SensorData&);
5238 
5239  private:
5244 
5249 
5254 
5259 
5261 
5262  friend class boost::serialization::access;
5263  template<class Archive>
5264  void serialize(Archive &ar, const unsigned int version)
5265  {
5266  ar & BOOST_SERIALIZATION_NVP(m_StateId);
5267  ar & BOOST_SERIALIZATION_NVP(m_UniqueId);
5268  ar & BOOST_SERIALIZATION_NVP(m_SensorName);
5269  ar & BOOST_SERIALIZATION_NVP(m_Time);
5270  ar & BOOST_SERIALIZATION_NVP(m_CustomData);
5271  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
5272  }
5273 };
5274 
5275 
5279 
5283  typedef std::vector<kt_double> RangeReadingsVector;
5284 
5289  {
5290  public:
5291  // @cond EXCLUDE
5293  // @endcond
5294 
5295  public:
5300  LaserRangeScan(const Name& rSensorName)
5301  : SensorData(rSensorName)
5302  , m_pRangeReadings(NULL)
5303  , m_NumberOfRangeReadings(0)
5304  {
5305  }
5306 
5308  {
5309  }
5310 
5316  LaserRangeScan(const Name& rSensorName, const RangeReadingsVector& rRangeReadings)
5317  : SensorData(rSensorName)
5318  , m_pRangeReadings(NULL)
5319  , m_NumberOfRangeReadings(0)
5320  {
5321  assert(rSensorName.ToString() != "");
5322 
5323  SetRangeReadings(rRangeReadings);
5324  }
5325 
5330  {
5331  delete [] m_pRangeReadings;
5332  m_pRangeReadings = nullptr;
5333  }
5334 
5335  public:
5340  inline kt_double* GetRangeReadings() const
5341  {
5342  return m_pRangeReadings;
5343  }
5344 
5346  {
5347  return RangeReadingsVector(m_pRangeReadings, m_pRangeReadings + m_NumberOfRangeReadings);
5348  }
5349 
5354  inline void SetRangeReadings(const RangeReadingsVector& rRangeReadings)
5355  {
5356  // ignore for now! XXXMAE BUGUBUG 05/21/2010 << TODO(lucbettaieb): What the heck is this??
5357  // if (rRangeReadings.size() != GetNumberOfRangeReadings())
5358  // {
5359  // std::stringstream error;
5360  // error << "Given number of readings (" << rRangeReadings.size()
5361  // << ") does not match expected number of range finder (" << GetNumberOfRangeReadings() << ")";
5362  // throw Exception(error.str());
5363  // }
5364 
5365  if (!rRangeReadings.empty())
5366  {
5367  if (rRangeReadings.size() != m_NumberOfRangeReadings)
5368  {
5369  // delete old readings
5370  delete [] m_pRangeReadings;
5371 
5372  // store size of array!
5373  m_NumberOfRangeReadings = static_cast<kt_int32u>(rRangeReadings.size());
5374 
5375  // allocate range readings
5376  m_pRangeReadings = new kt_double[m_NumberOfRangeReadings];
5377  }
5378 
5379  // copy readings
5380  kt_int32u index = 0;
5381  const_forEach(RangeReadingsVector, &rRangeReadings)
5382  {
5383  m_pRangeReadings[index++] = *iter;
5384  }
5385  }
5386  else
5387  {
5388  delete [] m_pRangeReadings;
5389  m_pRangeReadings = NULL;
5390  }
5391  }
5392 
5398  {
5399  return SensorManager::GetInstance()->GetSensorByName<LaserRangeFinder>(GetSensorName());
5400  }
5401 
5407  {
5408  return m_NumberOfRangeReadings;
5409  }
5410 
5411  private:
5413  const LaserRangeScan& operator=(const LaserRangeScan&);
5414 
5415  private:
5418 
5419  friend class boost::serialization::access;
5420  template<class Archive>
5421  void serialize(Archive &ar, const unsigned int version)
5422  {
5423  ar & BOOST_SERIALIZATION_NVP(m_NumberOfRangeReadings);
5424  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(SensorData);
5425 
5426  if (Archive::is_loading::value)
5427  {
5428  m_pRangeReadings = new kt_double[m_NumberOfRangeReadings];
5429  }
5430  ar & boost::serialization::make_array<kt_double>(m_pRangeReadings, m_NumberOfRangeReadings);
5431  }
5432  }; // LaserRangeScan
5433 
5437 
5441  class DrivePose : public SensorData
5442  {
5443  public:
5444  // @cond EXCLUDE
5446  // @endcond
5447 
5448  public:
5453  DrivePose(const Name& rSensorName)
5454  : SensorData(rSensorName)
5455  {
5456  }
5457 
5461  virtual ~DrivePose()
5462  {
5463  }
5464 
5465  public:
5470  inline const Pose3& GetOdometricPose() const
5471  {
5472  return m_OdometricPose;
5473  }
5474 
5479  inline void SetOdometricPose(const Pose3& rPose)
5480  {
5481  m_OdometricPose = rPose;
5482  }
5483 
5484  private:
5485  DrivePose(const DrivePose&);
5486  const DrivePose& operator=(const DrivePose&);
5487 
5488  private:
5493  }; // class DrivePose
5494 
5498 
5506  {
5507  public:
5508  // @cond EXCLUDE
5510  // @endcond
5511 
5512  public:
5516  LocalizedRangeScan(const Name& rSensorName, const RangeReadingsVector& rReadings)
5517  : LaserRangeScan(rSensorName, rReadings)
5518  , m_IsDirty(true)
5519  {
5520  }
5521 
5523  {}
5524 
5529  {
5530  }
5531 
5532  private:
5533  mutable std::shared_mutex m_Lock;
5534 
5535  public:
5540  inline const Pose2& GetOdometricPose() const
5541  {
5542  return m_OdometricPose;
5543  }
5544 
5549  inline void SetOdometricPose(const Pose2& rPose)
5550  {
5551  m_OdometricPose = rPose;
5552  }
5553 
5562  inline const Pose2& GetCorrectedPose() const
5563  {
5564  return m_CorrectedPose;
5565  }
5566 
5571  inline void SetCorrectedPose(const Pose2& rPose)
5572  {
5573  m_CorrectedPose = rPose;
5574 
5575  m_IsDirty = true;
5576  }
5577 
5582  inline void SetCorrectedPoseAndUpdate(const Pose2& rPose)
5583  {
5584  SetCorrectedPose(rPose);
5585 
5586  Update();
5587  }
5588 
5592  inline const Pose2& GetBarycenterPose() const
5593  {
5594  std::shared_lock<std::shared_mutex> lock(m_Lock);
5595  if (m_IsDirty)
5596  {
5597  // throw away constness and do an update!
5598  lock.unlock();
5599  std::unique_lock<std::shared_mutex> uniqueLock(m_Lock);
5600  const_cast<LocalizedRangeScan*>(this)->Update();
5601  }
5602 
5603  return m_BarycenterPose;
5604  }
5605 
5606  inline void SetBarycenterPose(Pose2& bcenter)
5607  {
5608  m_BarycenterPose = bcenter;
5609  }
5610 
5616  inline Pose2 GetReferencePose(kt_bool useBarycenter) const
5617  {
5618  std::shared_lock<std::shared_mutex> lock(m_Lock);
5619  if (m_IsDirty)
5620  {
5621  // throw away constness and do an update!
5622  lock.unlock();
5623  std::unique_lock<std::shared_mutex> uniqueLock(m_Lock);
5624  const_cast<LocalizedRangeScan*>(this)->Update();
5625  }
5626 
5627  return useBarycenter ? GetBarycenterPose() : GetSensorPose();
5628  }
5629 
5634  inline Pose2 GetSensorPose() const
5635  {
5636  return GetSensorAt(m_CorrectedPose);
5637  }
5638 
5639  inline void SetIsDirty(kt_bool& rIsDirty)
5640  {
5641  m_IsDirty = rIsDirty;
5642  }
5643 
5648  void SetSensorPose(const Pose2& rScanPose)
5649  {
5650  m_CorrectedPose = GetCorrectedAt(rScanPose);
5651 
5652  Update();
5653  }
5654 
5660  inline Pose2 GetSensorAt(const Pose2& rPose) const
5661  {
5662  return Transform(rPose).TransformPose(GetLaserRangeFinder()->GetOffsetPose());
5663  }
5664 
5670  inline Pose2 GetCorrectedAt(const Pose2& sPose) const
5671  {
5672  Pose2 deviceOffsetPose2 = GetLaserRangeFinder()->GetOffsetPose();
5673  kt_double offsetLength = deviceOffsetPose2.GetPosition().Length();
5674  kt_double offsetHeading = deviceOffsetPose2.GetHeading();
5675  kt_double angleoffset = atan2(deviceOffsetPose2.GetY(), deviceOffsetPose2.GetX());
5676  kt_double correctedHeading = math::NormalizeAngle(sPose.GetHeading());
5677  Pose2 worldSensorOffset = Pose2(offsetLength * cos(correctedHeading + angleoffset - offsetHeading),
5678  offsetLength * sin(correctedHeading + angleoffset - offsetHeading),
5679  offsetHeading);
5680 
5681  return sPose - worldSensorOffset;
5682  }
5683 
5688  inline const BoundingBox2& GetBoundingBox() const
5689  {
5690  std::shared_lock<std::shared_mutex> lock(m_Lock);
5691  if (m_IsDirty)
5692  {
5693  // throw away constness and do an update!
5694  lock.unlock();
5695  std::unique_lock<std::shared_mutex> uniqueLock(m_Lock);
5696  const_cast<LocalizedRangeScan*>(this)->Update();
5697  }
5698 
5699  return m_BoundingBox;
5700  }
5701 
5702  inline void SetBoundingBox(BoundingBox2& bbox)
5703  {
5704  m_BoundingBox = bbox;
5705  }
5706 
5710  inline const PointVectorDouble& GetPointReadings(kt_bool wantFiltered = false) const
5711  {
5712  std::shared_lock<std::shared_mutex> lock(m_Lock);
5713  if (m_IsDirty)
5714  {
5715  // throw away constness and do an update!
5716  lock.unlock();
5717  std::unique_lock<std::shared_mutex> uniqueLock(m_Lock);
5718  const_cast<LocalizedRangeScan*>(this)->Update();
5719  }
5720 
5721  if (wantFiltered == true)
5722  {
5723  return m_PointReadings;
5724  }
5725  else
5726  {
5727  return m_UnfilteredPointReadings;
5728  }
5729  }
5730 
5731  inline void SetPointReadings(PointVectorDouble& points, kt_bool setFiltered = false)
5732  {
5733  if (setFiltered)
5734  {
5735  m_PointReadings = points;
5736  }
5737  else
5738  {
5739  m_UnfilteredPointReadings = points;
5740  }
5741  }
5742 
5743  private:
5748  virtual void Update()
5749  {
5750  LaserRangeFinder* pLaserRangeFinder = GetLaserRangeFinder();
5751 
5752  if (pLaserRangeFinder != NULL)
5753  {
5754  m_PointReadings.clear();
5755  m_UnfilteredPointReadings.clear();
5756 
5757  kt_double rangeThreshold = pLaserRangeFinder->GetRangeThreshold();
5758  kt_double minimumAngle = pLaserRangeFinder->GetMinimumAngle();
5759  kt_double angularResolution = pLaserRangeFinder->GetAngularResolution();
5760  Pose2 scanPose = GetSensorPose();
5761 
5762  // compute point readings
5763  Vector2<kt_double> rangePointsSum;
5764  kt_int32u beamNum = 0;
5765  for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++)
5766  {
5767  kt_double rangeReading = GetRangeReadings()[i];
5768  if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold))
5769  {
5770  kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
5771 
5772  Vector2<kt_double> point;
5773  point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
5774  point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
5775 
5776  m_UnfilteredPointReadings.push_back(point);
5777  continue;
5778  }
5779 
5780  kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;
5781 
5782  Vector2<kt_double> point;
5783  point.SetX(scanPose.GetX() + (rangeReading * cos(angle)));
5784  point.SetY(scanPose.GetY() + (rangeReading * sin(angle)));
5785 
5786  m_PointReadings.push_back(point);
5787  m_UnfilteredPointReadings.push_back(point);
5788 
5789  rangePointsSum += point;
5790  }
5791 
5792  // compute barycenter
5793  kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
5794  if (nPoints != 0.0)
5795  {
5796  Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
5797  m_BarycenterPose = Pose2(averagePosition, 0.0);
5798  }
5799  else
5800  {
5801  m_BarycenterPose = scanPose;
5802  }
5803 
5804  // calculate bounding box of scan
5805  m_BoundingBox = BoundingBox2();
5806  m_BoundingBox.Add(scanPose.GetPosition());
5807  forEach(PointVectorDouble, &m_PointReadings)
5808  {
5809  m_BoundingBox.Add(*iter);
5810  }
5811  }
5812 
5813  m_IsDirty = false;
5814  }
5815 
5819  friend class boost::serialization::access;
5820  template<class Archive>
5821  void serialize(Archive &ar, const unsigned int version)
5822  {
5823  ar & BOOST_SERIALIZATION_NVP(m_OdometricPose);
5824  ar & BOOST_SERIALIZATION_NVP(m_CorrectedPose);
5825  ar & BOOST_SERIALIZATION_NVP(m_BarycenterPose);
5826  ar & BOOST_SERIALIZATION_NVP(m_PointReadings);
5827  ar & BOOST_SERIALIZATION_NVP(m_UnfilteredPointReadings);
5828  ar & BOOST_SERIALIZATION_NVP(m_BoundingBox);
5829  ar & BOOST_SERIALIZATION_NVP(m_IsDirty);
5830  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(LaserRangeScan);
5831  }
5832 
5833 
5834  private:
5836  const LocalizedRangeScan& operator=(const LocalizedRangeScan&);
5837 
5838  private:
5843 
5848 
5849  protected:
5854 
5859 
5864 
5869 
5874  }; // LocalizedRangeScan
5875 
5879  typedef std::vector<LocalizedRangeScan*> LocalizedRangeScanVector;
5880  typedef std::map<int, LocalizedRangeScan*> LocalizedRangeScanMap;
5884 
5889  {
5890  public:
5891  // @cond EXCLUDE
5893  // @endcond
5894 
5895  public:
5900  LocalizedRangeScanWithPoints(const Name& rSensorName, const RangeReadingsVector& rReadings,
5901  const PointVectorDouble& rPoints)
5902  : m_Points(rPoints),
5903  LocalizedRangeScan(rSensorName, rReadings)
5904  {
5905  }
5906 
5911  {
5912  }
5913 
5914  private:
5918  void Update()
5919  {
5920  m_PointReadings.clear();
5921  m_UnfilteredPointReadings.clear();
5922 
5923  Pose2 scanPose = GetSensorPose();
5924  Pose2 robotPose = GetCorrectedPose();
5925 
5926  // update point readings
5927  Vector2<kt_double> rangePointsSum;
5928  for (kt_int32u i = 0; i < m_Points.size(); i++)
5929  {
5930  // check if this has a NaN
5931  if (!std::isfinite(m_Points[i].GetX()) || !std::isfinite(m_Points[i].GetY()))
5932  {
5933  Vector2<kt_double> point(m_Points[i].GetX(), m_Points[i].GetY());
5934  m_UnfilteredPointReadings.push_back(point);
5935 
5936  continue;
5937  }
5938 
5939  // transform into world coords
5940  Pose2 pointPose(m_Points[i].GetX(), m_Points[i].GetY(), 0);
5941  Pose2 result = Transform(robotPose).TransformPose(pointPose);
5942  Vector2<kt_double> point(result.GetX(), result.GetY());
5943 
5944  m_PointReadings.push_back(point);
5945  m_UnfilteredPointReadings.push_back(point);
5946 
5947  rangePointsSum += point;
5948  }
5949 
5950  // compute barycenter
5951  kt_double nPoints = static_cast<kt_double>(m_PointReadings.size());
5952  if (nPoints != 0.0)
5953  {
5954  Vector2<kt_double> averagePosition = Vector2<kt_double>(rangePointsSum / nPoints);
5955  m_BarycenterPose = Pose2(averagePosition, 0.0);
5956  }
5957  else
5958  {
5959  m_BarycenterPose = scanPose;
5960  }
5961 
5962  // calculate bounding box of scan
5963  m_BoundingBox = BoundingBox2();
5964  m_BoundingBox.Add(scanPose.GetPosition());
5965  forEach(PointVectorDouble, &m_PointReadings)
5966  {
5967  m_BoundingBox.Add(*iter);
5968  }
5969 
5970  m_IsDirty = false;
5971  }
5972 
5973  private:
5976 
5977  private:
5979  }; // LocalizedRangeScanWithPoints
5980 
5984 
5985  class OccupancyGrid;
5986 
5988  {
5989  public:
5991  : m_pOccupancyGrid(pGrid)
5992  {
5993  }
5994 
5999  virtual void operator() (kt_int32u index);
6000 
6001  private:
6003  }; // CellUpdater
6004 
6008  class OccupancyGrid : public Grid<kt_int8u>
6009  {
6010  friend class CellUpdater;
6011  friend class IncrementalOccupancyGrid;
6012 
6013  public:
6021  OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2<kt_double>& rOffset, kt_double resolution)
6022  : Grid<kt_int8u>(width, height)
6023  , m_pCellPassCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
6024  , m_pCellHitsCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
6025  , m_pCellUpdater(NULL)
6026  {
6027  m_pCellUpdater = new CellUpdater(this);
6028 
6029  if (karto::math::DoubleEqual(resolution, 0.0))
6030  {
6031  throw Exception("Resolution cannot be 0");
6032  }
6033 
6034  m_pMinPassThrough = new Parameter<kt_int32u>("MinPassThrough", 2);
6035  m_pOccupancyThreshold = new Parameter<kt_double>("OccupancyThreshold", 0.1);
6036 
6037  GetCoordinateConverter()->SetScale(1.0 / resolution);
6038  GetCoordinateConverter()->SetOffset(rOffset);
6039  }
6040 
6044  virtual ~OccupancyGrid()
6045  {
6046  delete m_pCellUpdater;
6047 
6048  delete m_pCellPassCnt;
6049  delete m_pCellHitsCnt;
6050 
6051  delete m_pMinPassThrough;
6052  delete m_pOccupancyThreshold;
6053  }
6054 
6055  public:
6062  {
6063  if (rScans.empty())
6064  {
6065  return NULL;
6066  }
6067 
6068  kt_int32s width, height;
6069  Vector2<kt_double> offset;
6070  ComputeDimensions(rScans, resolution, width, height, offset);
6071  OccupancyGrid* pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);
6072  pOccupancyGrid->CreateFromScans(rScans);
6073 
6074  return pOccupancyGrid;
6075  }
6076 
6082  {
6083  OccupancyGrid* pOccupancyGrid = new OccupancyGrid(GetWidth(),
6084  GetHeight(),
6085  GetCoordinateConverter()->GetOffset(),
6086  1.0 / GetCoordinateConverter()->GetScale());
6087  memcpy(pOccupancyGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
6088 
6089  pOccupancyGrid->GetCoordinateConverter()->SetSize(GetCoordinateConverter()->GetSize());
6090  pOccupancyGrid->m_pCellPassCnt = m_pCellPassCnt->Clone();
6091  pOccupancyGrid->m_pCellHitsCnt = m_pCellHitsCnt->Clone();
6092 
6093  return pOccupancyGrid;
6094  }
6095 
6101  virtual kt_bool IsFree(const Vector2<kt_int32s>& rPose) const
6102  {
6103  kt_int8u* pOffsets = reinterpret_cast<kt_int8u*>(GetDataPointer(rPose));
6104  if (*pOffsets == GridStates_Free)
6105  {
6106  return true;
6107  }
6108 
6109  return false;
6110  }
6111 
6119  virtual kt_double RayCast(const Pose2& rPose2, kt_double maxRange) const
6120  {
6121  double scale = GetCoordinateConverter()->GetScale();
6122 
6123  kt_double x = rPose2.GetX();
6124  kt_double y = rPose2.GetY();
6125  kt_double theta = rPose2.GetHeading();
6126 
6127  kt_double sinTheta = sin(theta);
6128  kt_double cosTheta = cos(theta);
6129 
6130  kt_double xStop = x + maxRange * cosTheta;
6131  kt_double xSteps = 1 + fabs(xStop - x) * scale;
6132 
6133  kt_double yStop = y + maxRange * sinTheta;
6134  kt_double ySteps = 1 + fabs(yStop - y) * scale;
6135 
6136  kt_double steps = math::Maximum(xSteps, ySteps);
6137  kt_double delta = maxRange / steps;
6138  kt_double distance = delta;
6139 
6140  for (kt_int32u i = 1; i < steps; i++)
6141  {
6142  kt_double x1 = x + distance * cosTheta;
6143  kt_double y1 = y + distance * sinTheta;
6144 
6145  Vector2<kt_int32s> gridIndex = WorldToGrid(Vector2<kt_double>(x1, y1));
6146  if (IsValidGridIndex(gridIndex) && IsFree(gridIndex))
6147  {
6148  distance = (i + 1) * delta;
6149  }
6150  else
6151  {
6152  break;
6153  }
6154  }
6155 
6156  return (distance < maxRange)? distance : maxRange;
6157  }
6158 
6165  {
6166  m_pMinPassThrough->SetValue(count);
6167  }
6168 
6174  {
6175  m_pOccupancyThreshold->SetValue(thresh);
6176  }
6177 
6178  protected:
6184  {
6185  return m_pCellHitsCnt;
6186  }
6187 
6193  {
6194  return m_pCellPassCnt;
6195  }
6196 
6197  protected:
6206  static void ComputeDimensions(const LocalizedRangeScanVector& rScans,
6207  kt_double resolution,
6208  kt_int32s& rWidth,
6209  kt_int32s& rHeight,
6210  Vector2<kt_double>& rOffset)
6211  {
6212  BoundingBox2 boundingBox;
6213 
6215  {
6216  if (*iter == nullptr)
6217  {
6218  continue;
6219  }
6220 
6221  boundingBox.Add((*iter)->GetBoundingBox());
6222  }
6223 
6224  kt_double scale = 1.0 / resolution;
6225  Size2<kt_double> size = boundingBox.GetSize();
6226 
6227  rWidth = static_cast<kt_int32s>(math::Round(size.GetWidth() * scale));
6228  rHeight = static_cast<kt_int32s>(math::Round(size.GetHeight() * scale));
6229  rOffset = boundingBox.GetMinimum();
6230  }
6231 
6236  virtual void CreateFromScans(const LocalizedRangeScanVector& rScans)
6237  {
6238  m_pCellPassCnt->Resize(GetWidth(), GetHeight());
6239  m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
6240 
6241  m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
6242  m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
6243 
6245  {
6246  if (*iter == nullptr)
6247  {
6248  continue;
6249  }
6250 
6251  LocalizedRangeScan* pScan = *iter;
6252  AddScan(pScan);
6253  }
6254 
6255  Update();
6256  }
6257 
6265  virtual kt_bool AddScan(LocalizedRangeScan* pScan, kt_bool doUpdate = false)
6266  {
6267  LaserRangeFinder* laserRangeFinder = pScan->GetLaserRangeFinder();
6268  kt_double rangeThreshold = laserRangeFinder->GetRangeThreshold();
6269  kt_double maxRange = laserRangeFinder->GetMaximumRange();
6270  kt_double minRange = laserRangeFinder->GetMinimumRange();
6271 
6272  Vector2<kt_double> scanPosition = pScan->GetSensorPose().GetPosition();
6273  // get scan point readings
6274  const PointVectorDouble& rPointReadings = pScan->GetPointReadings(false);
6275 
6276  kt_bool isAllInMap = true;
6277 
6278  // draw lines from scan position to all point readings
6279  int pointIndex = 0;
6280  const_forEachAs(PointVectorDouble, &rPointReadings, pointsIter)
6281  {
6282  Vector2<kt_double> point = *pointsIter;
6283  kt_double rangeReading = pScan->GetRangeReadings()[pointIndex];
6284  kt_bool isEndPointValid = rangeReading < (rangeThreshold - KT_TOLERANCE);
6285 
6286  if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading))
6287  {
6288  // ignore these readings
6289  pointIndex++;
6290  continue;
6291  }
6292  else if (rangeReading >= rangeThreshold)
6293  {
6294  // trace up to range reading
6295  kt_double ratio = rangeThreshold / rangeReading;
6296  kt_double dx = point.GetX() - scanPosition.GetX();
6297  kt_double dy = point.GetY() - scanPosition.GetY();
6298  point.SetX(scanPosition.GetX() + ratio * dx);
6299  point.SetY(scanPosition.GetY() + ratio * dy);
6300  }
6301 
6302  kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate);
6303  if (!isInMap)
6304  {
6305  isAllInMap = false;
6306  }
6307 
6308  pointIndex++;
6309  }
6310 
6311  return isAllInMap;
6312  }
6313 
6323  virtual kt_bool RayTrace(const Vector2<kt_double>& rWorldFrom,
6324  const Vector2<kt_double>& rWorldTo,
6325  kt_bool isEndPointValid,
6326  kt_bool doUpdate = false)
6327  {
6328  assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
6329 
6330  Vector2<kt_int32s> gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom);
6331  Vector2<kt_int32s> gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo);
6332 
6333  CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL;
6334  m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater);
6335 
6336  // for the end point
6337  if (isEndPointValid)
6338  {
6339  if (m_pCellPassCnt->IsValidGridIndex(gridTo))
6340  {
6341  kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false);
6342 
6343  kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
6344  kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
6345 
6346  // increment cell pass through and hit count
6347  pCellPassCntPtr[index]++;
6348  pCellHitCntPtr[index]++;
6349 
6350  if (doUpdate)
6351  {
6352  (*m_pCellUpdater)(index);
6353  }
6354  }
6355  }
6356 
6357  return m_pCellPassCnt->IsValidGridIndex(gridTo);
6358  }
6359 
6366  virtual void UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
6367  {
6368  if (cellPassCnt > m_pMinPassThrough->GetValue())
6369  {
6370  kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);
6371 
6372  if (hitRatio > m_pOccupancyThreshold->GetValue())
6373  {
6374  *pCell = GridStates_Occupied;
6375  }
6376  else
6377  {
6378  *pCell = GridStates_Free;
6379  }
6380  }
6381  }
6382 
6386  virtual void Update()
6387  {
6388  assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
6389 
6390  // clear grid
6391  Clear();
6392 
6393  // set occupancy status of cells
6394  kt_int8u* pDataPtr = GetDataPointer();
6395  kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
6396  kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
6397 
6398  kt_int32u nBytes = GetDataSize();
6399  for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++)
6400  {
6401  UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);
6402  }
6403  }
6404 
6410  virtual void Resize(kt_int32s width, kt_int32s height)
6411  {
6412  Grid<kt_int8u>::Resize(width, height);
6413  m_pCellPassCnt->Resize(width, height);
6414  m_pCellHitsCnt->Resize(width, height);
6415  }
6416 
6417  protected:
6422 
6427 
6428  private:
6432  OccupancyGrid(const OccupancyGrid&);
6433 
6437  const OccupancyGrid& operator=(const OccupancyGrid&);
6438 
6439  private:
6441 
6443  // NOTE: These two values are dependent on the resolution. If the resolution is too small,
6444  // then not many beams will hit the cell!
6445 
6446  // Number of beams that must pass through a cell before it will be considered to be occupied
6447  // or unoccupied. This prevents stray beams from messing up the map.
6449 
6450  // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied
6452  }; // OccupancyGrid
6453 
6457 
6462  class DatasetInfo : public Object
6463  {
6464  public:
6465  // @cond EXCLUDE
6467  // @endcond
6468 
6469  public:
6471  : Object()
6472  {
6473  m_pTitle = new Parameter<std::string>("Title", "", GetParameterManager());
6474  m_pAuthor = new Parameter<std::string>("Author", "", GetParameterManager());
6475  m_pDescription = new Parameter<std::string>("Description", "", GetParameterManager());
6476  m_pCopyright = new Parameter<std::string>("Copyright", "", GetParameterManager());
6477  }
6478 
6479  virtual ~DatasetInfo()
6480  {
6481  }
6482 
6483  public:
6487  const std::string& GetTitle() const
6488  {
6489  return m_pTitle->GetValue();
6490  }
6491 
6495  const std::string& GetAuthor() const
6496  {
6497  return m_pAuthor->GetValue();
6498  }
6499 
6503  const std::string& GetDescription() const
6504  {
6505  return m_pDescription->GetValue();
6506  }
6507 
6511  const std::string& GetCopyright() const
6512  {
6513  return m_pCopyright->GetValue();
6514  }
6515 
6520  private:
6521  DatasetInfo(const DatasetInfo&);
6522  const DatasetInfo& operator=(const DatasetInfo&);
6523 
6524  private:
6529  friend class boost::serialization::access;
6530  template<class Archive>
6531  void serialize(Archive &ar, const unsigned int version)
6532  {
6533  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Object);
6534  ar & BOOST_SERIALIZATION_NVP(*m_pTitle);
6535  ar & BOOST_SERIALIZATION_NVP(*m_pAuthor);
6536  ar & BOOST_SERIALIZATION_NVP(*m_pDescription);
6537  ar & BOOST_SERIALIZATION_NVP(*m_pCopyright);
6538  }
6539  }; // class DatasetInfo
6540 
6544 
6549  class Dataset
6550  {
6551  public:
6556  : m_pDatasetInfo(NULL)
6557  {
6558  }
6559 
6563  virtual ~Dataset()
6564  {
6565  Clear();
6566  }
6567 
6572  void SaveToFile(const std::string& filename)
6573  {
6574  printf("Save To File\n");
6575  std::ofstream ofs(filename.c_str());
6576  boost::archive::binary_oarchive oa(ofs, boost::archive::no_codecvt);
6577  oa << BOOST_SERIALIZATION_NVP(*this);
6578  }
6579 
6584  void LoadFromFile(const std::string& filename)
6585  {
6586  printf("Load From File\n");
6587  std::ifstream ifs(filename.c_str());
6588  boost::archive::binary_iarchive ia(ifs, boost::archive::no_codecvt); //no second arg?
6589  ia >> BOOST_SERIALIZATION_NVP(*this);
6590  }
6591 
6592  public:
6597  void Add(Object* pObject, kt_bool overrideSensorName = false)
6598  {
6599  if (pObject != NULL)
6600  {
6601  if (dynamic_cast<Sensor*>(pObject))
6602  {
6603  Sensor* pSensor = dynamic_cast<Sensor*>(pObject);
6604  if (pSensor != NULL)
6605  {
6606  m_SensorNameLookup[pSensor->GetName()] = pSensor;
6607  karto::SensorManager::GetInstance()->RegisterSensor(pSensor, overrideSensorName);
6608  }
6609 
6610  m_Lasers.push_back(pObject);
6611  }
6612  else if (dynamic_cast<SensorData*>(pObject))
6613  {
6614  SensorData* pSensorData = dynamic_cast<SensorData*>(pObject);
6615  m_Data.insert({pSensorData->GetUniqueId(), pSensorData});
6616  }
6617  else if (dynamic_cast<DatasetInfo*>(pObject))
6618  {
6619  m_pDatasetInfo = dynamic_cast<DatasetInfo*>(pObject);
6620  }
6621  else
6622  {
6623  std::cout << "Did not save object of non-data and non-sensor type" << std::endl;
6624  }
6625  }
6626  }
6627 
6632  inline const ObjectVector& GetLasers() const
6633  {
6634  return m_Lasers;
6635  }
6636 
6641  inline const DataMap& GetData() const
6642  {
6643  return m_Data;
6644  }
6645 
6650  inline void RemoveData(LocalizedRangeScan* scan)
6651  {
6652  auto iterator = m_Data.find(scan->GetUniqueId());
6653  if (iterator != m_Data.end())
6654  {
6655  delete iterator->second;
6656  iterator->second = nullptr;
6657  m_Data.erase(iterator);
6658  }
6659  else
6660  {
6661  std::cout << "Failed to remove data. Pointer to LocalizedRangeScan could not be found in dataset. "
6662  << "Most likely different pointer address but same object TODO STEVE." << std::endl;
6663  }
6664  }
6665 
6671  {
6672  return m_pDatasetInfo;
6673  }
6674 
6678  virtual void Clear()
6679  {
6680  for (std::map<Name, Sensor*>::iterator iter = m_SensorNameLookup.begin(); iter != m_SensorNameLookup.end(); ++iter)
6681  {
6683  }
6684 
6685  forEach(ObjectVector, &m_Lasers)
6686  {
6687  if(*iter)
6688  {
6689  delete *iter;
6690  *iter = NULL;
6691  }
6692  }
6693 
6694  for (auto iter = m_Data.begin(); iter != m_Data.end(); ++iter)
6695  {
6696  if(iter->second)
6697  {
6698  delete iter->second;
6699  iter->second = NULL;
6700  }
6701  }
6702 
6703  m_Lasers.clear();
6704  m_Data.clear();
6705 
6706  if (m_pDatasetInfo != NULL)
6707  {
6708  delete m_pDatasetInfo;
6709  m_pDatasetInfo = NULL;
6710  }
6711  }
6712 
6713  private:
6714  std::map<Name, Sensor*> m_SensorNameLookup;
6721  friend class boost::serialization::access;
6722  template<class Archive>
6723  void serialize(Archive &ar, const unsigned int version)
6724  {
6725  std::cout<<"**Serializing Dataset**\n";
6726  std::cout<<"Dataset <- m_SensorNameLookup\n";
6727  ar & BOOST_SERIALIZATION_NVP(m_SensorNameLookup);
6728  std::cout<<"Dataset <- m_Data\n";
6729  ar & BOOST_SERIALIZATION_NVP(m_Data);
6730  std::cout<<"Dataset <- m_Lasers\n";
6731  ar & BOOST_SERIALIZATION_NVP(m_Lasers);
6732  std::cout<<"Dataset <- m_pDatasetInfo\n";
6733  ar & BOOST_SERIALIZATION_NVP(m_pDatasetInfo);
6734  std::cout<<"**Finished serializing Dataset**\n";
6735  }
6736 
6737  }; // Dataset
6738  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Dataset)
6742 
6748  {
6749  public:
6754  : m_pArray(NULL)
6755  , m_Capacity(0)
6756  , m_Size(0)
6757  {
6758  }
6759 
6763  virtual ~LookupArray()
6764  {
6765  assert(m_pArray != NULL);
6766 
6767  delete[] m_pArray;
6768  m_pArray = NULL;
6769  }
6770 
6771  public:
6775  void Clear()
6776  {
6777  memset(m_pArray, 0, sizeof(kt_int32s) * m_Capacity);
6778  }
6779 
6785  {
6786  return m_Size;
6787  }
6788 
6793  void SetSize(kt_int32u size)
6794  {
6795  assert(size != 0);
6796 
6797  if (size > m_Capacity)
6798  {
6799  if (m_pArray != NULL)
6800  {
6801  delete [] m_pArray;
6802  }
6803  m_Capacity = size;
6804  m_pArray = new kt_int32s[m_Capacity];
6805  }
6806 
6807  m_Size = size;
6808  }
6809 
6815  inline kt_int32s& operator [] (kt_int32u index)
6816  {
6817  assert(index < m_Size);
6818 
6819  return m_pArray[index];
6820  }
6821 
6827  inline kt_int32s operator [] (kt_int32u index) const
6828  {
6829  assert(index < m_Size);
6830 
6831  return m_pArray[index];
6832  }
6833 
6839  {
6840  return m_pArray;
6841  }
6842 
6847  inline kt_int32s* GetArrayPointer() const
6848  {
6849  return m_pArray;
6850  }
6851 
6852  private:
6856  friend class boost::serialization::access;
6857  template<class Archive>
6858  void serialize(Archive &ar, const unsigned int version)
6859  {
6860  ar & BOOST_SERIALIZATION_NVP(m_Capacity);
6861  ar & BOOST_SERIALIZATION_NVP(m_Size);
6862  if (Archive::is_loading::value)
6863  {
6864  m_pArray = new kt_int32s[m_Capacity];
6865  }
6866  ar & boost::serialization::make_array<kt_int32s >(m_pArray, m_Capacity);
6867 
6868 
6869  }
6870  }; // LookupArray
6871 
6875 
6889  template<typename T>
6891  {
6892  public:
6898  {
6899  }
6901  : m_pGrid(pGrid)
6902  , m_Capacity(0)
6903  , m_Size(0)
6904  , m_ppLookupArray(NULL)
6905  {
6906  }
6907 
6912  {
6913  DestroyArrays();
6914  }
6915 
6916  public:
6923  {
6924  assert(math::IsUpTo(index, m_Size));
6925 
6926  return m_ppLookupArray[index];
6927  }
6928 
6933  const std::vector<kt_double>& GetAngles() const
6934  {
6935  return m_Angles;
6936  }
6937 
6946  kt_double angleCenter,
6947  kt_double angleOffset,
6948  kt_double angleResolution)
6949  {
6950  assert(angleOffset != 0.0);
6951  assert(angleResolution != 0.0);
6952 
6953  kt_int32u nAngles = static_cast<kt_int32u>(math::Round(angleOffset * 2.0 / angleResolution) + 1);
6954  SetSize(nAngles);
6955 
6957  // convert points into local coordinates of scan pose
6958 
6959  const PointVectorDouble& rPointReadings = pScan->GetPointReadings();
6960 
6961  // compute transform to scan pose
6962  Transform transform(pScan->GetSensorPose());
6963 
6964  Pose2Vector localPoints;
6965  const_forEach(PointVectorDouble, &rPointReadings)
6966  {
6967  // do inverse transform to get points in local coordinates
6968  Pose2 vec = transform.InverseTransformPose(Pose2(*iter, 0.0));
6969  localPoints.push_back(vec);
6970  }
6971 
6973  // create lookup array for different angles
6974  kt_double angle = 0.0;
6975  kt_double startAngle = angleCenter - angleOffset;
6976  for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
6977  {
6978  angle = startAngle + angleIndex * angleResolution;
6979  ComputeOffsets(angleIndex, angle, localPoints, pScan);
6980  }
6981  // assert(math::DoubleEqual(angle, angleCenter + angleOffset));
6982  }
6983 
6984  private:
6991  void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2Vector& rLocalPoints, LocalizedRangeScan* pScan)
6992  {
6993  m_ppLookupArray[angleIndex]->SetSize(static_cast<kt_int32u>(rLocalPoints.size()));
6994  m_Angles.at(angleIndex) = angle;
6995 
6996  // set up point array by computing relative offsets to points readings
6997  // when rotated by given angle
6998 
6999  const Vector2<kt_double>& rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset();
7000 
7001  kt_double cosine = cos(angle);
7002  kt_double sine = sin(angle);
7003 
7004  kt_int32u readingIndex = 0;
7005 
7006  kt_int32s* pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer();
7007 
7008  kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
7009 
7010  const_forEach(Pose2Vector, &rLocalPoints)
7011  {
7012  const Vector2<kt_double>& rPosition = iter->GetPosition();
7013 
7014  if (std::isnan(pScan->GetRangeReadings()[readingIndex]) || std::isinf(pScan->GetRangeReadings()[readingIndex]))
7015  {
7016  pAngleIndexPointer[readingIndex] = INVALID_SCAN;
7017  readingIndex++;
7018  continue;
7019  }
7020 
7021 
7022  // counterclockwise rotation and that rotation is about the origin (0, 0).
7023  Vector2<kt_double> offset;
7024  offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY());
7025  offset.SetY(sine * rPosition.GetX() + cosine * rPosition.GetY());
7026 
7027  // have to compensate for the grid offset when getting the grid index
7028  Vector2<kt_int32s> gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset);
7029 
7030  // use base GridIndex to ignore ROI
7031  kt_int32s lookupIndex = m_pGrid->Grid<T>::GridIndex(gridPoint, false);
7032 
7033  pAngleIndexPointer[readingIndex] = lookupIndex;
7034 
7035  readingIndex++;
7036  }
7037  assert(readingIndex == rLocalPoints.size());
7038  }
7039 
7044  void SetSize(kt_int32u size)
7045  {
7046  assert(size != 0);
7047 
7048  if (size > m_Capacity)
7049  {
7050  if (m_ppLookupArray != NULL)
7051  {
7052  DestroyArrays();
7053  }
7054 
7055  m_Capacity = size;
7056  m_ppLookupArray = new LookupArray*[m_Capacity];
7057  for (kt_int32u i = 0; i < m_Capacity; i++)
7058  {
7059  m_ppLookupArray[i] = new LookupArray();
7060  }
7061  }
7062 
7063  m_Size = size;
7064 
7065  m_Angles.resize(size);
7066  }
7067 
7072  {
7073  if (m_ppLookupArray)
7074  {
7075  for (kt_int32u i = 0; i < m_Capacity; i++)
7076  {
7077  delete m_ppLookupArray[i];
7078  }
7079  }
7080  if (m_ppLookupArray)
7081  {
7082  delete[] m_ppLookupArray;
7083  m_ppLookupArray = NULL;
7084  }
7085  }
7086 
7087  private:
7089 
7092 
7094 
7095  // for sanity check
7096  std::vector<kt_double> m_Angles;
7097  friend class boost::serialization::access;
7098  template<class Archive>
7099  void serialize(Archive &ar, const unsigned int version)
7100  {
7101  ar & BOOST_SERIALIZATION_NVP(m_pGrid);
7102  ar & BOOST_SERIALIZATION_NVP(m_Capacity);
7103  ar & BOOST_SERIALIZATION_NVP(m_Size);
7104  ar & BOOST_SERIALIZATION_NVP(m_Angles);
7105  if (Archive::is_loading::value)
7106  {
7107  m_ppLookupArray = new LookupArray*[m_Capacity];
7108  for (kt_int32u i = 0; i < m_Capacity; i++)
7109  {
7110  m_ppLookupArray[i] = new LookupArray();
7111  }
7112  }
7113  ar & boost::serialization::make_array<LookupArray*>(m_ppLookupArray, m_Capacity);
7114  }
7115  }; // class GridIndexLookup
7116 
7120 
7121  inline Pose2::Pose2(const Pose3& rPose)
7122  : m_Position(rPose.GetPosition().GetX(), rPose.GetPosition().GetY())
7123  {
7124  kt_double t1, t2;
7125 
7126  // calculates heading from orientation
7127  rPose.GetOrientation().ToEulerAngles(m_Heading, t1, t2);
7128  }
7129 
7133 
7134  // @cond EXCLUDE
7135 
7136  template<typename T>
7137  inline void Object::SetParameter(const std::string& rName, T value)
7138  {
7139  AbstractParameter* pParameter = GetParameter(rName);
7140  if (pParameter != NULL)
7141  {
7142  std::stringstream stream;
7143  stream << value;
7144  pParameter->SetValueFromString(stream.str());
7145  }
7146  else
7147  {
7148  throw Exception("Parameter does not exist: " + rName);
7149  }
7150  }
7151 
7152  template<>
7153  inline void Object::SetParameter(const std::string& rName, kt_bool value)
7154  {
7155  AbstractParameter* pParameter = GetParameter(rName);
7156  if (pParameter != NULL)
7157  {
7158  pParameter->SetValueFromString(value ? "true" : "false");
7159  }
7160  else
7161  {
7162  throw Exception("Parameter does not exist: " + rName);
7163  }
7164  }
7165 
7166  // @endcond
7167 
7169 }; // namespace karto
7170 
7198 
7199 #endif // karto_sdk_KARTO_H
karto::OccupancyGrid::RayCast
virtual kt_double RayCast(const Pose2 &rPose2, kt_double maxRange) const
Definition: Karto.h:6119
karto::Exception::Exception
Exception(const Exception &rException)
Definition: Karto.h:116
karto::Name
Definition: Karto.h:389
const_forEach
#define const_forEach(listtype, list)
Definition: Macros.h:72
karto::AbstractParameter::AbstractParameter
AbstractParameter()
Definition: Karto.h:3107
karto::math::Round
kt_double Round(kt_double value)
Definition: Math.h:87
karto::Name::operator<<
friend std::ostream & operator<<(std::ostream &rStream, const Name &rName)
Definition: Karto.h:531
karto::Vector2::operator+=
void operator+=(const Vector2 &rOther)
Definition: Karto.h:1121
karto::Vector2::operator>>
friend std::istream & operator>>(std::istream &rStream, const Vector2 &)
Definition: Karto.h:1262
karto::ParameterEnum::~ParameterEnum
virtual ~ParameterEnum()
Definition: Karto.h:3433
karto::Rectangle2::m_Size
Size2< T > m_Size
Definition: Karto.h:2024
karto::CoordinateConverter::m_Size
Size2< kt_int32s > m_Size
Definition: Karto.h:4619
karto::Vector3::m_Values
T m_Values[3]
Definition: Karto.h:1551
karto::Pose2::SquaredDistance
kt_double SquaredDistance(const Pose2 &rOther) const
Definition: Karto.h:2171
karto::LaserRangeFinder::GetMinimumRange
kt_double GetMinimumRange() const
Definition: Karto.h:3945
karto::Singleton
Definition: Karto.h:211
karto::Quaternion::GetX
kt_double GetX() const
Definition: Karto.h:1624
karto::Dataset::GetLasers
const ObjectVector & GetLasers() const
Definition: Karto.h:6632
karto::LaserRangeFinder::m_pMaximumRange
Parameter< kt_double > * m_pMaximumRange
Definition: Karto.h:4390
karto::KT_TOLERANCE
const kt_double KT_TOLERANCE
Definition: Math.h:41
karto::BoundingBox2::access
friend class boost::serialization::access
Definition: Karto.h:2956
karto::Transform::TransformPose
Pose2 TransformPose(const Pose2 &rSourcePose)
Definition: Karto.h:3004
karto::Size2::Size2
Size2()
Definition: Karto.h:878
karto::Quaternion::operator=
Quaternion & operator=(const Quaternion &rQuaternion)
Definition: Karto.h:1762
karto::Vector2::operator!=
kt_bool operator!=(const Vector2 &rOther) const
Definition: Karto.h:1227
karto::Rectangle2
Definition: Karto.h:1823
karto::Name::SetName
void SetName(const std::string &rName)
Definition: Karto.h:436
karto::LocalizedRangeScan::GetBoundingBox
const BoundingBox2 & GetBoundingBox() const
Definition: Karto.h:5688
karto::Pose2::m_Heading
kt_double m_Heading
Definition: Karto.h:2265
karto::Grid::~Grid
virtual ~Grid()
Definition: Karto.h:4667
karto::SensorManager::RegisterSensor
void RegisterSensor(Sensor *pSensor, kt_bool override=false)
Definition: Karto.h:3732
karto::Vector2::Vector2
Vector2()
Definition: Karto.h:1006
karto::Size2::operator!=
kt_bool operator!=(const Size2 &rOther) const
Definition: Karto.h:964
karto::LookupArray::m_Capacity
kt_int32u m_Capacity
Definition: Karto.h:6854
karto::Vector2::operator-
const Vector2 operator-(const Vector2 &rOther) const
Definition: Karto.h:1151
karto::ParameterEnum::SetValueFromString
virtual void SetValueFromString(const std::string &rStringValue)
Definition: Karto.h:3455
karto::Pose2::Pose2
Pose2(kt_double x, kt_double y, kt_double heading)
Definition: Karto.h:2074
karto::LaserRangeFinder::Update
void Update()
Definition: Karto.h:4365
karto::Pose3::Pose3
Pose3()
Definition: Karto.h:2290
karto::ParameterEnum::Clone
virtual Parameter< kt_int32s > * Clone()
Definition: Karto.h:3442
karto::OccupancyGrid::m_pOccupancyThreshold
Parameter< kt_double > * m_pOccupancyThreshold
Definition: Karto.h:6451
karto::GridIndexLookup::ComputeOffsets
void ComputeOffsets(LocalizedRangeScan *pScan, kt_double angleCenter, kt_double angleOffset, kt_double angleResolution)
Definition: Karto.h:6945
karto::BoundingBox2::SetMinimum
void SetMinimum(const Vector2< kt_double > &mMinimum)
Definition: Karto.h:2893
karto::Pose2Vector
std::vector< Pose2 > Pose2Vector
Definition: Karto.h:2271
karto::Grid::Grid
Grid()
Definition: Karto.h:4652
karto::Pose2::Pose2
Pose2(const Vector2< kt_double > &rPosition, kt_double heading)
Definition: Karto.h:2062
karto::LaserRangeScan::GetLaserRangeFinder
LaserRangeFinder * GetLaserRangeFinder() const
Definition: Karto.h:5397
karto::Size2::m_Width
T m_Width
Definition: Karto.h:981
karto::Dataset::Add
void Add(Object *pObject, kt_bool overrideSensorName=false)
Definition: Karto.h:6597
karto::Vector2::Distance
kt_double Distance(const Vector2 &rOther) const
Definition: Karto.h:1112
karto::LaserRangeFinder::SetAngularResolution
void SetAngularResolution(kt_double angularResolution)
Definition: Karto.h:4058
karto::LookupArray::m_pArray
kt_int32s * m_pArray
Definition: Karto.h:6853
karto::Object::GetName
const Name & GetName() const
Definition: Karto.h:658
karto::Vector3::operator==
kt_bool operator==(const Vector3 &rOther) const
Definition: Karto.h:1511
ObjectType_None
const kt_objecttype ObjectType_None
Definition: Karto.h:67
karto::Grid::GetWidthStep
kt_int32s GetWidthStep() const
Definition: Karto.h:4870
kt_double
double kt_double
Definition: Types.h:67
Macros.h
karto::NonCopyable
Definition: Karto.h:181
karto::Rectangle2::Rectangle2
Rectangle2(const Rectangle2 &rOther)
Definition: Karto.h:1860
karto::CoordinateConverter::WorldToGrid
Vector2< kt_int32s > WorldToGrid(const Vector2< kt_double > &rWorld, kt_bool flipY=false) const
Definition: Karto.h:4488
karto::Parameter::m_Value
T m_Value
Definition: Karto.h:3354
karto::LocalizedRangeScan::GetOdometricPose
const Pose2 & GetOdometricPose() const
Definition: Karto.h:5540
karto::LaserRangeFinder
Definition: Karto.h:3922
karto::LocalizedRangeScan::GetReferencePose
Pose2 GetReferencePose(kt_bool useBarycenter) const
Definition: Karto.h:5616
karto::Vector2::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:1270
karto::CoordinateConverter::SetSize
void SetSize(const Size2< kt_int32s > &rSize)
Definition: Karto.h:4568
karto::LocalizedRangeScanWithPoints::m_Points
const PointVectorDouble m_Points
Definition: Karto.h:5978
angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
karto::Pose2::GetPosition
const Vector2< kt_double > & GetPosition() const
Definition: Karto.h:2135
karto::SensorManager::GetSensorByName
Sensor * GetSensorByName(const Name &rName)
Definition: Karto.h:3773
karto::NonCopyable::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:198
karto::CoordinateConverter::m_Scale
kt_double m_Scale
Definition: Karto.h:4620
karto::Name::access
friend class boost::serialization::access
Definition: Karto.h:618
karto::LocalizedRangeScan::m_BarycenterPose
Pose2 m_BarycenterPose
Definition: Karto.h:5853
karto::LocalizedRangeScan::GetSensorPose
Pose2 GetSensorPose() const
Definition: Karto.h:5634
karto::Pose2::SetPosition
void SetPosition(const Vector2< kt_double > &rPosition)
Definition: Karto.h:2144
karto::OccupancyGrid::AddScan
virtual kt_bool AddScan(LocalizedRangeScan *pScan, kt_bool doUpdate=false)
Definition: Karto.h:6265
karto::LaserRangeScan::SetRangeReadings
void SetRangeReadings(const RangeReadingsVector &rRangeReadings)
Definition: Karto.h:5354
karto::OccupancyGrid::GetCellPassCounts
virtual Grid< kt_int32u > * GetCellPassCounts()
Definition: Karto.h:6192
karto::IsParameters
kt_bool IsParameters(Object *pObject)
Definition: Karto.h:796
karto::AbstractParameter::AbstractParameter
AbstractParameter(const std::string &rName, ParameterManager *pParameterManger=NULL)
Definition: Karto.h:3115
karto::Object::GetParameters
const ParameterVector & GetParameters() const
Definition: Karto.h:706
karto::Rectangle2::GetX
T GetX() const
Definition: Karto.h:1871
karto::Quaternion::SetX
void SetX(kt_double x)
Definition: Karto.h:1633
karto::Parameter::SetValue
void SetValue(const T &rValue)
Definition: Karto.h:3282
karto::LaserRangeFinder::~LaserRangeFinder
virtual ~LaserRangeFinder()
Definition: Karto.h:3936
karto::Matrix3::InverseFast
kt_bool InverseFast(Matrix3 &rkInverse, kt_double fTolerance=KT_TOLERANCE) const
Definition: Karto.h:2560
karto::AbstractParameter::Clone
virtual AbstractParameter * Clone()=0
karto::Dataset::SaveToFile
void SaveToFile(const std::string &filename)
Definition: Karto.h:6572
karto::ParameterManager::GetParameterVector
const ParameterVector & GetParameterVector() const
Definition: Karto.h:340
karto::LocalizedRangeScan::m_BoundingBox
BoundingBox2 m_BoundingBox
Definition: Karto.h:5868
karto::IsLocalizedRangeScanWithPoints
kt_bool IsLocalizedRangeScanWithPoints(Object *pObject)
Definition: Karto.h:786
karto::Transform::access
friend class boost::serialization::access
Definition: Karto.h:3067
karto::Module
Definition: Karto.h:818
karto::BoundingBox2::BoundingBox2
BoundingBox2()
Definition: Karto.h:2875
karto::OccupancyGrid
Definition: Karto.h:6008
karto::CellUpdater::CellUpdater
CellUpdater(OccupancyGrid *pGrid)
Definition: Karto.h:5990
karto::LookupArray::~LookupArray
virtual ~LookupArray()
Definition: Karto.h:6763
karto::Quaternion::SetW
void SetW(kt_double w)
Definition: Karto.h:1687
karto::OccupancyGrid::GetCellHitsCounts
virtual Grid< kt_int32u > * GetCellHitsCounts()
Definition: Karto.h:6183
karto::Matrix3::operator<<
friend std::ostream & operator<<(std::ostream &rStream, const Matrix3 &rMatrix)
Definition: Karto.h:2708
karto::Pose3::operator<<
friend std::ostream & operator<<(std::ostream &rStream, const Pose3 &rPose)
Definition: Karto.h:2416
karto::Quaternion::FromEulerAngles
void FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll)
Definition: Karto.h:1736
karto::Rectangle2::SetY
void SetY(T y)
Definition: Karto.h:1898
karto::IsLocalizedRangeScan
kt_bool IsLocalizedRangeScan(Object *pObject)
Definition: Karto.h:776
karto::GridStates
GridStates
Definition: Karto.h:4444
karto::Pose2::access
friend class boost::serialization::access
Definition: Karto.h:2254
karto::Name::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:620
karto::Exception::GetErrorMessage
const std::string & GetErrorMessage() const
Definition: Karto.h:146
karto::Matrix3::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:2718
karto::math::DegreesToRadians
kt_double DegreesToRadians(kt_double degrees)
Definition: Math.h:56
karto::Pose2::GetY
kt_double GetY() const
Definition: Karto.h:2117
karto::Vector3::GetZ
const T & GetZ() const
Definition: Karto.h:1371
karto::DatasetInfo::GetTitle
const std::string & GetTitle() const
Definition: Karto.h:6487
karto::LaserRangeFinder::m_NumberOfRangeReadings
kt_int32u m_NumberOfRangeReadings
Definition: Karto.h:4398
karto::BoundingBox2::Add
void Add(const BoundingBox2 &rBoundingBox)
Definition: Karto.h:2936
karto::Quaternion::m_Values
kt_double m_Values[4]
Definition: Karto.h:1811
karto::Name::ToString
std::string ToString() const
Definition: Karto.h:469
karto::Object::GetObjectType
virtual kt_objecttype GetObjectType() const =0
karto::LocalizedRangeScan::m_UnfilteredPointReadings
PointVectorDouble m_UnfilteredPointReadings
Definition: Karto.h:5863
karto::Name::IsValidFirst
kt_bool IsValidFirst(char c)
Definition: Karto.h:597
karto::CoordinateConverter::m_Offset
Vector2< kt_double > m_Offset
Definition: Karto.h:4622
karto::AbstractParameter::~AbstractParameter
virtual ~AbstractParameter()
Definition: Karto.h:3147
karto::LookupArray::Clear
void Clear()
Definition: Karto.h:6775
karto::Drive::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3896
karto::Dataset::m_SensorNameLookup
std::map< Name, Sensor * > m_SensorNameLookup
Definition: Karto.h:6714
karto::Sensor::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3677
karto::Vector3::operator-
const Vector3 operator-(const Vector3 &rOther) const
Definition: Karto.h:1481
karto::CoordinateConverter::GetResolution
kt_double GetResolution() const
Definition: Karto.h:4586
karto::SensorManager::SensorManager
SensorManager()
Definition: Karto.h:3707
karto::GridIndexLookup::m_Capacity
kt_int32u m_Capacity
Definition: Karto.h:7090
karto::Pose3::Pose3
Pose3(const Pose2 &rPose)
Definition: Karto.h:2326
karto::Matrix::GetColumns
kt_int32u GetColumns() const
Definition: Karto.h:2780
karto::Pose2::operator-
Pose2 operator-(const Pose2 &rOther) const
Definition: Karto.h:2228
karto::OccupancyGrid::IsFree
virtual kt_bool IsFree(const Vector2< kt_int32s > &rPose) const
Definition: Karto.h:6101
karto::Object::m_Name
Name m_Name
Definition: Karto.h:715
karto::LaserRangeScan::LaserRangeScan
LaserRangeScan(const Name &rSensorName)
Definition: Karto.h:5300
karto::Vector3::MakeFloor
void MakeFloor(const Vector3 &rOther)
Definition: Karto.h:1389
karto::OccupancyGrid::OccupancyGrid
OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2< kt_double > &rOffset, kt_double resolution)
Definition: Karto.h:6021
karto::ParameterManager::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:370
karto::GridIndexLookup::m_pGrid
Grid< T > * m_pGrid
Definition: Karto.h:7088
karto::LookupArray
Definition: Karto.h:6747
karto::Grid::Clone
Grid * Clone()
Definition: Karto.h:4692
karto::LaserRangeFinder::GetRangeThreshold
kt_double GetRangeThreshold() const
Definition: Karto.h:3985
karto::LookupArray::m_Size
kt_int32u m_Size
Definition: Karto.h:6855
karto::LocalizedRangeScan::GetBarycenterPose
const Pose2 & GetBarycenterPose() const
Definition: Karto.h:5592
karto::SensorManager::GetSensorByName
T * GetSensorByName(const Name &rName)
Definition: Karto.h:3789
karto::LocalizedRangeScan::SetOdometricPose
void SetOdometricPose(const Pose2 &rPose)
Definition: Karto.h:5549
karto::Dataset::~Dataset
virtual ~Dataset()
Definition: Karto.h:6563
karto::LocalizedRangeScan::SetPointReadings
void SetPointReadings(PointVectorDouble &points, kt_bool setFiltered=false)
Definition: Karto.h:5731
karto::LaserRangeFinder::GetMaximumAngle
kt_double GetMaximumAngle() const
Definition: Karto.h:4029
karto::Vector2::GetX
const T & GetX() const
Definition: Karto.h:1028
karto::LocalizedRangeScan::m_OdometricPose
Pose2 m_OdometricPose
Definition: Karto.h:5842
karto::Matrix3::Transpose
Matrix3 Transpose() const
Definition: Karto.h:2527
karto::CoordinateConverter::SetScale
void SetScale(kt_double scale)
Definition: Karto.h:4541
karto::Transform::m_InverseRotation
Matrix3 m_InverseRotation
Definition: Karto.h:3065
karto::Vector3::SetY
void SetY(const T &y)
Definition: Karto.h:1362
karto::Size2::SetHeight
void SetHeight(T height)
Definition: Karto.h:937
karto::Vector2::m_Values
T m_Values[2]
Definition: Karto.h:1277
karto::Pose2::Pose2
Pose2()
Definition: Karto.h:2052
karto::LocalizedRangeScan::LocalizedRangeScan
LocalizedRangeScan()
Definition: Karto.h:5522
karto::LookupArray::LookupArray
LookupArray()
Definition: Karto.h:6753
karto::Exception::GetErrorCode
kt_int32s GetErrorCode()
Definition: Karto.h:155
karto::Pose2::operator<<
friend std::ostream & operator<<(std::ostream &rStream, const Pose2 &rPose)
Definition: Karto.h:2248
karto::Pose2::operator!=
kt_bool operator!=(const Pose2 &rOther) const
Definition: Karto.h:2199
ObjectType_Camera
const kt_objecttype ObjectType_Camera
Definition: Karto.h:75
karto::Grid::GetDataPointer
T * GetDataPointer()
Definition: Karto.h:4879
karto::Vector2::operator+
const Vector2 operator+(const Vector2 &rOther) const
Definition: Karto.h:1141
ObjectType_SensorData
const kt_objecttype ObjectType_SensorData
Definition: Karto.h:69
karto::Grid::Resize
virtual void Resize(kt_int32s width, kt_int32s height)
Definition: Karto.h:4707
karto::Matrix::Matrix
Matrix(kt_int32u rows, kt_int32u columns)
Definition: Karto.h:2737
ObjectType_Module
const kt_objecttype ObjectType_Module
Definition: Karto.h:86
karto::Vector2::MakeCeil
void MakeCeil(const Vector2 &rOther)
Definition: Karto.h:1074
karto::Vector3::Length
kt_double Length() const
Definition: Karto.h:1420
karto::Vector3::GetX
const T & GetX() const
Definition: Karto.h:1335
karto::SensorData::GetCustomData
const CustomDataVector & GetCustomData() const
Definition: Karto.h:5217
karto::LocalizedRangeScan
Definition: Karto.h:5505
karto::Matrix3::operator()
kt_double & operator()(kt_int32u row, kt_int32u column)
Definition: Karto.h:2631
karto::CustomData::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:5106
karto::Parameter::Parameter
Parameter()
Definition: Karto.h:3236
karto::SensorManager::~SensorManager
virtual ~SensorManager()
Definition: Karto.h:3714
karto::Parameter::Parameter
Parameter(const std::string &rName, const std::string &rDescription, T value, ParameterManager *pParameterManger=NULL)
Definition: Karto.h:3252
karto::Vector3::ToString
std::string ToString() const
Definition: Karto.h:1429
karto::LocalizedRangeScan::SetIsDirty
void SetIsDirty(kt_bool &rIsDirty)
Definition: Karto.h:5639
karto::Pose3::SetOrientation
void SetOrientation(const Quaternion &rOrientation)
Definition: Karto.h:2364
karto::LookupArray::GetArrayPointer
kt_int32s * GetArrayPointer()
Definition: Karto.h:6838
karto::ParameterManager::Get
AbstractParameter * Get(const std::string &rName)
Definition: Karto.h:319
karto::Quaternion::Quaternion
Quaternion(const Quaternion &rQuaternion)
Definition: Karto.h:1611
karto::Matrix3::FromAxisAngle
void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
Definition: Karto.h:2492
karto::Quaternion::ToEulerAngles
void ToEulerAngles(kt_double &rYaw, kt_double &rPitch, kt_double &rRoll) const
Definition: Karto.h:1699
karto::LaserRangeFinder_Hokuyo_URG_04LX
@ LaserRangeFinder_Hokuyo_URG_04LX
Definition: Karto.h:3093
karto::Parameter::Parameter
Parameter(const std::string &rName, T value, ParameterManager *pParameterManger=NULL)
Definition: Karto.h:3239
karto::AbstractParameter::GetValueAsString
virtual const std::string GetValueAsString() const =0
karto::Quaternion::operator<<
friend std::ostream & operator<<(std::ostream &rStream, const Quaternion &rQuaternion)
Definition: Karto.h:1801
karto::Rectangle2::SetX
void SetX(T x)
Definition: Karto.h:1880
ObjectType_LocalizedRangeScanWithPoints
const kt_objecttype ObjectType_LocalizedRangeScanWithPoints
Definition: Karto.h:81
karto::LocalizedRangeScan::GetPointReadings
const PointVectorDouble & GetPointReadings(kt_bool wantFiltered=false) const
Definition: Karto.h:5710
karto::Matrix3::operator()
kt_double operator()(kt_int32u row, kt_int32u column) const
Definition: Karto.h:2642
karto::LaserRangeFinder_Sick_LMS200
@ LaserRangeFinder_Sick_LMS200
Definition: Karto.h:3089
karto::OccupancyGrid::RayTrace
virtual kt_bool RayTrace(const Vector2< kt_double > &rWorldFrom, const Vector2< kt_double > &rWorldTo, kt_bool isEndPointValid, kt_bool doUpdate=false)
Definition: Karto.h:6323
karto::DatasetInfo::DatasetInfo
DatasetInfo()
Definition: Karto.h:6470
karto::SensorData::m_SensorName
Name m_SensorName
Definition: Karto.h:5253
karto::Grid::TraceLine
void TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor *f=NULL)
Definition: Karto.h:4949
karto::Grid::m_Width
kt_int32s m_Width
Definition: Karto.h:5030
karto::ParameterManager::m_Parameters
ParameterVector m_Parameters
Definition: Karto.h:365
KARTO_Object
#define KARTO_Object(name)
Definition: Karto.h:61
karto::Matrix3::Clear
void Clear()
Definition: Karto.h:2480
ObjectType_Drive
const kt_objecttype ObjectType_Drive
Definition: Karto.h:73
karto::Parameters
Definition: Karto.h:3552
karto::LocalizedRangeScan::SetBoundingBox
void SetBoundingBox(BoundingBox2 &bbox)
Definition: Karto.h:5702
karto::LocalizedRangeScanWithPoints
Definition: Karto.h:5888
karto::Grid::GetDataPointer
T * GetDataPointer(const Vector2< kt_int32s > &rGrid) const
Definition: Karto.h:4833
karto::Grid::Grid
Grid(kt_int32s width, kt_int32s height)
Definition: Karto.h:5022
karto::Size2
Definition: Karto.h:872
karto::DatasetInfo::m_pCopyright
Parameter< std::string > * m_pCopyright
Definition: Karto.h:6528
karto::CoordinateConverter::GetBoundingBox
BoundingBox2 GetBoundingBox() const
Definition: Karto.h:4604
karto::Rectangle2::GetCenter
const Vector2< T > GetCenter() const
Definition: Karto.h:1989
karto::Pose3::m_Orientation
Quaternion m_Orientation
Definition: Karto.h:2434
kt_int32s
int32_t kt_int32s
Definition: Types.h:51
karto::Grid::GridIndex
virtual kt_int32s GridIndex(const Vector2< kt_int32s > &rGrid, kt_bool boundaryCheck=true) const
Definition: Karto.h:4757
karto::LocalizedRangeScanWithPoints::LocalizedRangeScanWithPoints
LocalizedRangeScanWithPoints(const Name &rSensorName, const RangeReadingsVector &rReadings, const PointVectorDouble &rPoints)
Definition: Karto.h:5900
karto::math::Maximum
const T & Maximum(const T &value1, const T &value2)
Definition: Math.h:111
karto::LocalizedRangeScan::SetSensorPose
void SetSensorPose(const Pose2 &rScanPose)
Definition: Karto.h:5648
karto::Vector3::SquaredLength
kt_double SquaredLength() const
Definition: Karto.h:1411
karto::SensorData
Definition: Karto.h:5125
karto::Matrix3::SetToIdentity
void SetToIdentity()
Definition: Karto.h:2467
karto::SensorVector
std::vector< Sensor * > SensorVector
Definition: Karto.h:3687
karto::Rectangle2::GetPosition
const Vector2< T > & GetPosition() const
Definition: Karto.h:1943
karto::Name::GetName
const std::string & GetName() const
Definition: Karto.h:427
karto::Name::Parse
void Parse(const std::string &rName)
Definition: Karto.h:542
kt_objecttype
kt_int32u kt_objecttype
Definition: Karto.h:65
karto::DatasetInfo::m_pDescription
Parameter< std::string > * m_pDescription
Definition: Karto.h:6527
karto::Transform::SetTransform
void SetTransform(const Pose2 &rPose1, const Pose2 &rPose2)
Definition: Karto.h:3032
karto::Object::operator=
const Object & operator=(const Object &)
Read
Read
karto::CoordinateConverter::CoordinateConverter
CoordinateConverter()
Definition: Karto.h:4466
f
f
test
karto::LocalizedRangeScan::m_CorrectedPose
Pose2 m_CorrectedPose
Definition: Karto.h:5847
karto::CoordinateConverter::Transform
kt_double Transform(kt_double value)
Definition: Karto.h:4477
karto::Matrix::operator()
kt_double & operator()(kt_int32u row, kt_int32u column)
Definition: Karto.h:2791
karto::SensorData::SetUniqueId
void SetUniqueId(kt_int32u uniqueId)
Definition: Karto.h:5172
karto::Parameter::GetValueAsString
virtual const std::string GetValueAsString() const
Definition: Karto.h:3291
karto::Size2::operator==
kt_bool operator==(const Size2 &rOther) const
Definition: Karto.h:956
karto::CoordinateConverter::SetResolution
void SetResolution(kt_double resolution)
Definition: Karto.h:4595
karto::OccupancyGrid::m_pCellHitsCnt
Grid< kt_int32u > * m_pCellHitsCnt
Definition: Karto.h:6426
karto::OccupancyGrid::CreateFromScans
virtual void CreateFromScans(const LocalizedRangeScanVector &rScans)
Definition: Karto.h:6236
karto::Matrix3
Definition: Karto.h:2444
karto::LookupArray::SetSize
void SetSize(kt_int32u size)
Definition: Karto.h:6793
karto::Name::Name
Name(const Name &rOther)
Definition: Karto.h:410
karto::Matrix::RangeCheck
void RangeCheck(kt_int32u row, kt_int32u column) const
Definition: Karto.h:2842
karto::Vector3::SetZ
void SetZ(const T &z)
Definition: Karto.h:1380
karto::Name::operator!=
kt_bool operator!=(const Name &rOther) const
Definition: Karto.h:513
karto::LocalizedRangeScanWithPoints::~LocalizedRangeScanWithPoints
virtual ~LocalizedRangeScanWithPoints()
Definition: Karto.h:5910
ObjectType_Misc
const kt_objecttype ObjectType_Misc
Definition: Karto.h:71
karto::Matrix::m_Columns
kt_int32u m_Columns
Definition: Karto.h:2857
karto::Grid
Definition: Karto.h:4642
karto::LaserRangeFinder::m_pType
ParameterEnum * m_pType
Definition: Karto.h:4396
karto::IsLaserRangeFinder
kt_bool IsLaserRangeFinder(Object *pObject)
Definition: Karto.h:766
karto::Matrix3::operator+=
void operator+=(const Matrix3 &rkMatrix)
Definition: Karto.h:2692
karto::Quaternion::SetZ
void SetZ(kt_double z)
Definition: Karto.h:1669
karto::Sensor::m_pOffsetPose
Parameter< Pose2 > * m_pOffsetPose
Definition: Karto.h:3674
karto::OccupancyGrid::m_pMinPassThrough
Parameter< kt_int32u > * m_pMinPassThrough
Definition: Karto.h:6448
karto::GridIndexLookup::SetSize
void SetSize(kt_int32u size)
Definition: Karto.h:7044
karto::Vector2::operator/
const Vector2 operator/(T scalar) const
Definition: Karto.h:1171
karto::LocalizedRangeScan::GetSensorAt
Pose2 GetSensorAt(const Pose2 &rPose) const
Definition: Karto.h:5660
karto::DatasetInfo::GetCopyright
const std::string & GetCopyright() const
Definition: Karto.h:6511
karto::math::Clip
const T & Clip(const T &n, const T &minValue, const T &maxValue)
Definition: Math.h:124
karto::LaserRangeFinder_Sick_LMS100
@ LaserRangeFinder_Sick_LMS100
Definition: Karto.h:3088
karto::Pose3::m_Position
Vector3< kt_double > m_Position
Definition: Karto.h:2433
karto::Matrix3::Inverse
Matrix3 Inverse() const
Definition: Karto.h:2545
ObjectType_LaserRangeFinder
const kt_objecttype ObjectType_LaserRangeFinder
Definition: Karto.h:74
karto::LocalizedRangeScan::Update
virtual void Update()
Definition: Karto.h:5748
karto::LaserRangeFinder::m_pIs360Laser
Parameter< kt_bool > * m_pIs360Laser
Definition: Karto.h:4394
karto::LaserRangeFinder::m_pMaximumAngle
Parameter< kt_double > * m_pMaximumAngle
Definition: Karto.h:4385
karto::Functor::operator()
virtual void operator()(kt_int32u)
Definition: Karto.h:272
karto::GridIndexLookup::GridIndexLookup
GridIndexLookup()
Definition: Karto.h:6897
karto::Pose2::Pose2
Pose2(const Pose2 &rOther)
Definition: Karto.h:2088
ObjectType_Parameters
const kt_objecttype ObjectType_Parameters
Definition: Karto.h:84
karto::Grid::GetCoordinateConverter
CoordinateConverter * GetCoordinateConverter() const
Definition: Karto.h:4917
karto::Grid::CreateGrid
static Grid * CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
Definition: Karto.h:4655
karto::OccupancyGrid::ComputeDimensions
static void ComputeDimensions(const LocalizedRangeScanVector &rScans, kt_double resolution, kt_int32s &rWidth, kt_int32s &rHeight, Vector2< kt_double > &rOffset)
Definition: Karto.h:6206
karto::Parameters::~Parameters
virtual ~Parameters()
Definition: Karto.h:3575
karto::Matrix3::operator=
Matrix3 & operator=(const Matrix3 &rOther)
Definition: Karto.h:2619
karto::BoundingBox2::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:2958
karto::LaserRangeFinder::SetMinimumRange
void SetMinimumRange(kt_double minimumRange)
Definition: Karto.h:3954
karto::Singleton::operator=
const Singleton & operator=(const Singleton &)
karto::Pose2::m_Position
Vector2< kt_double > m_Position
Definition: Karto.h:2263
karto::Matrix
Definition: Karto.h:2731
karto::OccupancyGrid::CreateFromScans
static OccupancyGrid * CreateFromScans(const LocalizedRangeScanVector &rScans, kt_double resolution)
Definition: Karto.h:6061
karto::Pose3::Pose3
Pose3(const Pose3 &rOther)
Definition: Karto.h:2317
karto::OccupancyGrid::SetOccupancyThreshold
void SetOccupancyThreshold(kt_double thresh)
Definition: Karto.h:6173
karto::SensorData::m_Time
kt_double m_Time
Definition: Karto.h:5258
karto::Dataset::GetData
const DataMap & GetData() const
Definition: Karto.h:6641
karto::Pose2::operator=
Pose2 & operator=(const Pose2 &rOther)
Definition: Karto.h:2180
karto::Matrix::operator()
const kt_double & operator()(kt_int32u row, kt_int32u column) const
Definition: Karto.h:2804
karto::Transform::m_Rotation
Matrix3 m_Rotation
Definition: Karto.h:3064
karto::OccupancyGrid::UpdateCell
virtual void UpdateCell(kt_int8u *pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
Definition: Karto.h:6366
karto::CoordinateConverter::GetScale
kt_double GetScale() const
Definition: Karto.h:4532
karto::math::NormalizeAngle
kt_double NormalizeAngle(kt_double angle)
Definition: Math.h:182
karto::Dataset::m_Data
DataMap m_Data
Definition: Karto.h:6716
karto::SensorData::GetStateId
kt_int32s GetStateId() const
Definition: Karto.h:5145
karto::Matrix3::m_Matrix
kt_double m_Matrix[3][3]
Definition: Karto.h:2715
karto::Sensor::SetOffsetPose
void SetOffsetPose(const Pose2 &rPose)
Definition: Karto.h:3641
karto::DatasetInfo::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:6531
karto::Size2::Size2
Size2(const Size2 &rOther)
Definition: Karto.h:899
karto::ParameterEnum::ParameterEnum
ParameterEnum()
Definition: Karto.h:3426
Math.h
karto::Vector2::GetY
const T & GetY() const
Definition: Karto.h:1046
karto::Pose2::operator==
kt_bool operator==(const Pose2 &rOther) const
Definition: Karto.h:2191
karto::Quaternion::operator==
kt_bool operator==(const Quaternion &rOther) const
Definition: Karto.h:1776
karto::LaserRangeFinder::GetMinimumAngle
kt_double GetMinimumAngle() const
Definition: Karto.h:4009
karto::SensorManager::GetInstance
static SensorManager * GetInstance()
Definition: Karto.cpp:57
karto::Quaternion::GetZ
kt_double GetZ() const
Definition: Karto.h:1660
karto::DrivePose::m_OdometricPose
Pose3 m_OdometricPose
Definition: Karto.h:5492
karto::Transform::Transform
Transform(const Pose2 &rPose1, const Pose2 &rPose2)
Definition: Karto.h:2993
karto::Grid::GetDataPointer
T * GetDataPointer(const Vector2< kt_int32s > &rGrid)
Definition: Karto.h:4822
karto::Size2::GetHeight
const T GetHeight() const
Definition: Karto.h:928
karto::Grid::GetSize
const Size2< kt_int32s > GetSize() const
Definition: Karto.h:4861
karto::Dataset
Definition: Karto.h:6549
karto::LaserRangeFinder_Sick_LMS291
@ LaserRangeFinder_Sick_LMS291
Definition: Karto.h:3090
KARTO_EXPORT
#define KARTO_EXPORT
Definition: Macros.h:56
ObjectType_Sensor
const kt_objecttype ObjectType_Sensor
Definition: Karto.h:68
karto::LocalizedRangeScanVector
std::vector< LocalizedRangeScan * > LocalizedRangeScanVector
Definition: Karto.h:5879
karto::Exception::Exception
Exception(const std::string &rMessage="Karto Exception", kt_int32s errorCode=0)
Definition: Karto.h:107
karto::LaserRangeScan::m_NumberOfRangeReadings
kt_int32u m_NumberOfRangeReadings
Definition: Karto.h:5417
karto::Name::m_Scope
std::string m_Scope
Definition: Karto.h:614
karto::AbstractParameter::AbstractParameter
AbstractParameter(const std::string &rName, const std::string &rDescription, ParameterManager *pParameterManger=NULL)
Definition: Karto.h:3131
karto::LaserRangeFinder::SetIs360Laser
void SetIs360Laser(bool is_360_laser)
Definition: Karto.h:4144
karto::SensorData::m_CustomData
CustomDataVector m_CustomData
Definition: Karto.h:5260
karto::LaserRangeFinder::LaserRangeFinder
LaserRangeFinder()
Definition: Karto.h:3925
karto::ParameterEnum::GetValueAsString
virtual const std::string GetValueAsString() const
Definition: Karto.h:3478
karto::Parameter
Definition: Karto.h:3227
karto::Grid::GetResolution
kt_double GetResolution() const
Definition: Karto.h:4926
karto::ParameterManager::ParameterManager
ParameterManager()
Definition: Karto.h:295
karto::BoundingBox2::GetSize
Size2< kt_double > GetSize() const
Definition: Karto.h:2917
karto::LocalizedRangeScanWithPoints::Update
void Update()
Definition: Karto.h:5918
karto::Pose3::SetPosition
void SetPosition(const Vector3< kt_double > &rPosition)
Definition: Karto.h:2346
NormalizeAngle
T NormalizeAngle(const T &angle_radians)
Definition: ceres_utils.h:24
karto::SensorManagerMap
std::map< Name, Sensor * > SensorManagerMap
Definition: Karto.h:3696
karto::Exception::~Exception
virtual ~Exception()
Definition: Karto.h:125
karto::Rectangle2::operator!=
kt_bool operator!=(const Rectangle2 &rOther) const
Definition: Karto.h:2017
karto::Parameters::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3581
karto::LocalizedRangeScan::SetBarycenterPose
void SetBarycenterPose(Pose2 &bcenter)
Definition: Karto.h:5606
karto::NonCopyable::NonCopyable
NonCopyable()
Definition: Karto.h:188
karto::CoordinateConverter::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:4625
karto::LaserRangeFinder::SetRangeThreshold
void SetRangeThreshold(kt_double rangeThreshold)
Definition: Karto.h:3994
karto::Vector3::operator=
Vector3 & operator=(const Vector3 &rOther)
Definition: Karto.h:1443
karto::Pose2::SetY
void SetY(kt_double y)
Definition: Karto.h:2126
karto::Vector3::operator+
const Vector3 operator+(const Vector3 &rOther) const
Definition: Karto.h:1457
karto::Vector2::operator==
kt_bool operator==(const Vector2 &rOther) const
Definition: Karto.h:1218
karto::Singleton::Get
T * Get()
Definition: Karto.h:234
karto::CoordinateConverter
Definition: Karto.h:4460
karto::BoundingBox2::Add
void Add(const Vector2< kt_double > &rPoint)
Definition: Karto.h:2927
karto::Dataset::GetDatasetInfo
DatasetInfo * GetDatasetInfo()
Definition: Karto.h:6670
karto::SensorData::SetStateId
void SetStateId(kt_int32s stateId)
Definition: Karto.h:5154
karto::Matrix3::access
friend class boost::serialization::access
Definition: Karto.h:2716
karto::Sensor::GetOffsetPose
const Pose2 & GetOffsetPose() const
Definition: Karto.h:3632
karto::Sensor::Validate
virtual kt_bool Validate()=0
karto::LaserRangeScan::GetNumberOfRangeReadings
kt_int32u GetNumberOfRangeReadings() const
Definition: Karto.h:5406
karto::Vector2::operator*=
void operator*=(T scalar)
Definition: Karto.h:1208
karto::Exception::m_ErrorCode
kt_int32s m_ErrorCode
Definition: Karto.h:170
karto::LaserRangeFinder::SetMaximumRange
void SetMaximumRange(kt_double maximumRange)
Definition: Karto.h:3974
const_forEachAs
#define const_forEachAs(listtype, list, iter)
Definition: Macros.h:75
karto::OccupancyGrid::Resize
virtual void Resize(kt_int32s width, kt_int32s height)
Definition: Karto.h:6410
karto::Transform::InverseTransformPose
Pose2 InverseTransformPose(const Pose2 &rSourcePose)
Definition: Karto.h:3017
karto::LocalizedRangeScan::SetCorrectedPose
void SetCorrectedPose(const Pose2 &rPose)
Definition: Karto.h:5571
karto::Vector3::SetX
void SetX(const T &x)
Definition: Karto.h:1344
karto::GridIndexLookup::GetLookupArray
const LookupArray * GetLookupArray(kt_int32u index) const
Definition: Karto.h:6922
karto::Grid::GetWidth
kt_int32s GetWidth() const
Definition: Karto.h:4843
karto::Object::SetParameter
void SetParameter(const std::string &rName, T value)
Write
Write
karto::LaserRangeFinder::m_pMinimumRange
Parameter< kt_double > * m_pMinimumRange
Definition: Karto.h:4389
ObjectType_LocalizedRangeScan
const kt_objecttype ObjectType_LocalizedRangeScan
Definition: Karto.h:79
karto::AbstractParameter::m_Description
std::string m_Description
Definition: Karto.h:3205
karto::Singleton::~Singleton
virtual ~Singleton()
Definition: Karto.h:225
karto::Grid::IsValidGridIndex
kt_bool IsValidGridIndex(const Vector2< kt_int32s > &rGrid) const
Definition: Karto.h:4746
karto::LocalizedRangeScan::GetCorrectedPose
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5562
karto::Vector3::operator>>
friend std::istream & operator>>(std::istream &rStream, const Vector3 &)
Definition: Karto.h:1544
karto::Sensor::operator=
const Sensor & operator=(const Sensor &)
karto::Vector3::operator!=
kt_bool operator!=(const Vector3 &rOther) const
Definition: Karto.h:1522
karto::LaserRangeScan::LaserRangeScan
LaserRangeScan(const Name &rSensorName, const RangeReadingsVector &rRangeReadings)
Definition: Karto.h:5316
karto::CustomDataVector
std::vector< CustomData * > CustomDataVector
Definition: Karto.h:5116
karto::GridIndexLookup::DestroyArrays
void DestroyArrays()
Definition: Karto.h:7071
karto::LocalizedRangeScan::SetCorrectedPoseAndUpdate
void SetCorrectedPoseAndUpdate(const Pose2 &rPose)
Definition: Karto.h:5582
karto::Object::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:722
karto::Drive::~Drive
virtual ~Drive()
Definition: Karto.h:3871
karto::Rectangle2::GetHeight
T GetHeight() const
Definition: Karto.h:1925
karto::LookupArray::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:6858
karto::Pose3::operator==
kt_bool operator==(const Pose3 &rOther) const
Definition: Karto.h:2398
karto::CoordinateConverter::GridToWorld
Vector2< kt_double > GridToWorld(const Vector2< kt_int32s > &rGrid, kt_bool flipY=false) const
Definition: Karto.h:4511
karto::LaserRangeFinder_Hokuyo_UTM_30LX
@ LaserRangeFinder_Hokuyo_UTM_30LX
Definition: Karto.h:3092
karto::Rectangle2::SetHeight
void SetHeight(T height)
Definition: Karto.h:1934
kt_bool
bool kt_bool
Definition: Types.h:64
karto::Name::Name
Name(const std::string &rName)
Definition: Karto.h:402
karto::Parameter::operator=
Parameter & operator=(const Parameter &rOther)
Definition: Karto.h:3322
karto::GridIndexLookup::GridIndexLookup
GridIndexLookup(Grid< T > *pGrid)
Definition: Karto.h:6900
karto::Name::Validate
void Validate(const std::string &rName)
Definition: Karto.h:567
karto::BoundingBox2::SetMaximum
void SetMaximum(const Vector2< kt_double > &rMaximum)
Definition: Karto.h:2909
karto::Vector2::operator-=
void operator-=(const Vector2 &rOther)
Definition: Karto.h:1130
ObjectType_DrivePose
const kt_objecttype ObjectType_DrivePose
Definition: Karto.h:77
karto::Quaternion::SetY
void SetY(kt_double y)
Definition: Karto.h:1651
karto::DrivePose
Definition: Karto.h:5441
ObjectType_CustomData
const kt_objecttype ObjectType_CustomData
Definition: Karto.h:70
karto::LaserRangeFinder::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:4403
karto::LaserRangeFinder::m_pMinimumAngle
Parameter< kt_double > * m_pMinimumAngle
Definition: Karto.h:4384
karto::SensorData::m_UniqueId
kt_int32s m_UniqueId
Definition: Karto.h:5248
karto::LaserRangeFinder::GetMaximumRange
kt_double GetMaximumRange() const
Definition: Karto.h:3965
karto::CoordinateConverter::GetSize
const Size2< kt_int32s > & GetSize() const
Definition: Karto.h:4577
karto::Drive::Validate
virtual kt_bool Validate(SensorData *pSensorData)
Definition: Karto.h:3881
karto::Parameter::SetValueFromString
virtual void SetValueFromString(const std::string &rStringValue)
Definition: Karto.h:3302
karto::CoordinateConverter::SetOffset
void SetOffset(const Vector2< kt_double > &rOffset)
Definition: Karto.h:4559
karto::AbstractParameter::operator<<
friend std::ostream & operator<<(std::ostream &rStream, const AbstractParameter &rParameter)
Definition: Karto.h:3194
karto::Size2::m_Height
T m_Height
Definition: Karto.h:982
karto::SensorData::m_StateId
kt_int32s m_StateId
Definition: Karto.h:5243
karto::Matrix3::Matrix3
Matrix3(const Matrix3 &rOther)
Definition: Karto.h:2458
karto::Parameters::Parameters
Parameters()
Definition: Karto.h:3564
karto::Pose3
Definition: Karto.h:2284
karto::Pose3::Pose3
Pose3(const Vector3< kt_double > &rPosition)
Definition: Karto.h:2298
karto::Pose3::operator!=
kt_bool operator!=(const Pose3 &rOther) const
Definition: Karto.h:2406
karto::ParameterEnum
Definition: Karto.h:3411
karto::GridStates_Free
@ GridStates_Free
Definition: Karto.h:4448
karto::Size2::GetWidth
const T GetWidth() const
Definition: Karto.h:910
karto::LaserRangeScan::m_pRangeReadings
kt_double * m_pRangeReadings
Definition: Karto.h:5416
karto::LaserRangeScan::GetRangeReadings
kt_double * GetRangeReadings() const
Definition: Karto.h:5340
karto::Size2::SetWidth
void SetWidth(T width)
Definition: Karto.h:919
karto::SensorManager::Validate
static void Validate(Sensor *pSensor)
Definition: Karto.h:3817
karto::math::InRange
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:172
karto::CellUpdater::m_pOccupancyGrid
OccupancyGrid * m_pOccupancyGrid
Definition: Karto.h:6002
karto::PointVectorDouble
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1283
karto::AbstractParameter::m_Name
std::string m_Name
Definition: Karto.h:3204
karto::Name::GetScope
const std::string & GetScope() const
Definition: Karto.h:451
karto::Grid::IndexToGrid
Vector2< kt_int32s > IndexToGrid(kt_int32s index) const
Definition: Karto.h:4785
karto::OccupancyGrid::~OccupancyGrid
virtual ~OccupancyGrid()
Definition: Karto.h:6044
karto::LaserRangeFinder::Validate
virtual kt_bool Validate()
Definition: Karto.h:4152
karto::KT_PI_2
const kt_double KT_PI_2
Definition: Math.h:34
karto::Pose3::GetPosition
const Vector3< kt_double > & GetPosition() const
Definition: Karto.h:2337
karto::Vector2
Definition: Karto.h:1000
karto::Vector3::Vector3
Vector3(T x, T y, T z)
Definition: Karto.h:1312
karto::LookupArray::GetSize
kt_int32u GetSize() const
Definition: Karto.h:6784
karto::SensorManager::m_Sensors
SensorManagerMap m_Sensors
Definition: Karto.h:3833
karto::LaserRangeFinder::GetNumberOfRangeReadings
kt_int32u GetNumberOfRangeReadings() const
Definition: Karto.h:4125
karto::AbstractParameter
Definition: Karto.h:3103
karto::Size2::operator=
Size2 & operator=(const Size2 &rOther)
Definition: Karto.h:945
karto::NonCopyable::~NonCopyable
virtual ~NonCopyable()
Definition: Karto.h:192
karto::GridIndexLookup::GetAngles
const std::vector< kt_double > & GetAngles() const
Definition: Karto.h:6933
karto::Dataset::Dataset
Dataset()
Definition: Karto.h:6555
karto::GridIndexLookup::m_Size
kt_int32u m_Size
Definition: Karto.h:7091
karto::Grid::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:5039
karto::AbstractParameter::GetDescription
const std::string & GetDescription() const
Definition: Karto.h:3165
karto::LaserRangeFinder::LaserRangeFinder
LaserRangeFinder(const Name &rName)
Definition: Karto.h:4334
karto::Rectangle2::Rectangle2
Rectangle2(T x, T y, T width, T height)
Definition: Karto.h:1840
karto::math::DoubleEqual
kt_bool DoubleEqual(kt_double a, kt_double b)
Definition: Math.h:135
karto::Grid::m_pData
T * m_pData
Definition: Karto.h:5033
karto::BoundingBox2::GetMinimum
const Vector2< kt_double > & GetMinimum() const
Definition: Karto.h:2885
karto::ParameterManager::~ParameterManager
virtual ~ParameterManager()
Definition: Karto.h:302
karto::Matrix::Allocate
void Allocate()
Definition: Karto.h:2815
BOOST_CLASS_EXPORT_KEY
BOOST_CLASS_EXPORT_KEY(karto::NonCopyable)
karto::Parameter::Clone
virtual Parameter * Clone()
Definition: Karto.h:3313
karto::LocalizedRangeScan::m_Lock
std::shared_mutex m_Lock
Definition: Karto.h:5533
karto::Vector2::SetX
void SetX(const T &x)
Definition: Karto.h:1037
karto::Grid::GetValue
T GetValue(const Vector2< kt_int32s > &rGrid) const
Definition: Karto.h:4907
karto::GridIndexLookup::m_ppLookupArray
LookupArray ** m_ppLookupArray
Definition: Karto.h:7093
karto::AbstractParameter::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3211
karto::Object::GetParameterManager
virtual ParameterManager * GetParameterManager()
Definition: Karto.h:679
karto::ParameterEnum::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3538
karto::Exception
Definition: Karto.h:99
karto::Grid::GetDataSize
kt_int32s GetDataSize() const
Definition: Karto.h:4897
karto::ParameterEnum::DefineEnumValue
void DefineEnumValue(kt_int32s value, const std::string &rName)
Definition: Karto.h:3496
karto::AbstractParameter::GetName
const std::string & GetName() const
Definition: Karto.h:3156
karto::CustomData::~CustomData
virtual ~CustomData()
Definition: Karto.h:5083
karto::SensorManager::UnregisterSensor
void UnregisterSensor(Sensor *pSensor)
Definition: Karto.h:3752
karto::OccupancyGrid::SetMinPassThrough
void SetMinPassThrough(kt_int32u count)
Definition: Karto.h:6164
karto::Transform
Definition: Karto.h:2976
karto::Pose3::GetOrientation
const Quaternion & GetOrientation() const
Definition: Karto.h:2355
karto::LocalizedRangeScan::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:5821
karto::CoordinateConverter::GetOffset
const Vector2< kt_double > & GetOffset() const
Definition: Karto.h:4550
karto::Name::SetScope
void SetScope(const std::string &rScope)
Definition: Karto.h:460
karto::LocalizedRangeScan::m_IsDirty
kt_bool m_IsDirty
Definition: Karto.h:5873
karto::Drive::Drive
Drive(const std::string &rName)
Definition: Karto.h:3864
karto::SensorData::GetUniqueId
kt_int32s GetUniqueId() const
Definition: Karto.h:5163
karto::Vector2::operator/=
void operator/=(T scalar)
Definition: Karto.h:1160
karto::Dataset::LoadFromFile
void LoadFromFile(const std::string &filename)
Definition: Karto.h:6584
karto::OccupancyGrid::m_pCellUpdater
CellUpdater * m_pCellUpdater
Definition: Karto.h:6440
karto::GridIndexLookup::ComputeOffsets
void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2Vector &rLocalPoints, LocalizedRangeScan *pScan)
Definition: Karto.h:6991
karto::Name::operator==
kt_bool operator==(const Name &rOther) const
Definition: Karto.h:505
karto::Grid::m_WidthStep
kt_int32s m_WidthStep
Definition: Karto.h:5032
std
karto::Matrix::m_pData
kt_double * m_pData
Definition: Karto.h:2859
karto::Quaternion::Quaternion
Quaternion()
Definition: Karto.h:1585
karto::Parameter::~Parameter
virtual ~Parameter()
Definition: Karto.h:3264
karto::Rectangle2::SetWidth
void SetWidth(T width)
Definition: Karto.h:1916
karto::Dataset::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:6723
karto::Vector3::GetY
const T & GetY() const
Definition: Karto.h:1353
karto::Transform::m_Transform
Pose2 m_Transform
Definition: Karto.h:3062
karto::Dataset::m_Lasers
ObjectVector m_Lasers
Definition: Karto.h:6715
karto::Transform::Transform
Transform(const Pose2 &rPose)
Definition: Karto.h:2983
karto::Vector2::operator<
kt_bool operator<(const Vector2 &rOther) const
Definition: Karto.h:1237
karto::LaserRangeFinder::SetMaximumAngle
void SetMaximumAngle(kt_double maximumAngle)
Definition: Karto.h:4038
karto::Object::m_pParameterManager
ParameterManager * m_pParameterManager
Definition: Karto.h:716
karto::DrivePose::DrivePose
DrivePose(const Name &rSensorName)
Definition: Karto.h:5453
karto::DataMap
std::map< kt_int32s, Object * > DataMap
Definition: Karto.h:735
ObjectType_DatasetInfo
const kt_objecttype ObjectType_DatasetInfo
Definition: Karto.h:85
karto::CustomData
Definition: Karto.h:5064
karto::ParameterEnum::ParameterEnum
ParameterEnum(const std::string &rName, kt_int32s value, ParameterManager *pParameterManger=NULL)
Definition: Karto.h:3422
karto::math::Square
T Square(T value)
Definition: Math.h:77
karto::Dataset::Clear
virtual void Clear()
Definition: Karto.h:6678
kt_int32u
uint32_t kt_int32u
Definition: Types.h:52
karto::Size2::operator<<
friend std::ostream & operator<<(std::ostream &rStream, const Size2 &rSize)
Definition: Karto.h:974
karto::Pose2::GetX
kt_double GetX() const
Definition: Karto.h:2099
karto::Rectangle2::GetWidth
T GetWidth() const
Definition: Karto.h:1907
karto::Quaternion
Definition: Karto.h:1579
karto::SensorManager::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3837
karto::Rectangle2::SetPosition
void SetPosition(const Vector2< T > &rPosition)
Definition: Karto.h:1962
karto::Matrix::Clear
void Clear()
Definition: Karto.h:2759
karto::Vector3::Vector3
Vector3(const Vector3 &rOther)
Definition: Karto.h:1323
karto::Matrix3::ToString
std::string ToString() const
Definition: Karto.h:2599
karto::Name::~Name
virtual ~Name()
Definition: Karto.h:418
karto::IsDatasetInfo
kt_bool IsDatasetInfo(Object *pObject)
Definition: Karto.h:806
karto::Pose2::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:2256
ObjectType_CameraImage
const kt_objecttype ObjectType_CameraImage
Definition: Karto.h:80
karto::GridIndexLookup::m_Angles
std::vector< kt_double > m_Angles
Definition: Karto.h:7096
karto::Pose3::Pose3
Pose3(const Vector3< kt_double > &rPosition, const karto::Quaternion &rOrientation)
Definition: Karto.h:2308
karto::Matrix::~Matrix
virtual ~Matrix()
Definition: Karto.h:2750
karto::Grid::Clear
void Clear()
Definition: Karto.h:4683
karto::BoundingBox2::IsInBounds
kt_bool IsInBounds(const Vector2< kt_double > &rPoint) const
Definition: Karto.h:2947
karto::Pose2::SetHeading
void SetHeading(kt_double heading)
Definition: Karto.h:2162
karto::LaserRangeFinder::m_pAngularResolution
Parameter< kt_double > * m_pAngularResolution
Definition: Karto.h:4387
karto::CellUpdater
Definition: Karto.h:5987
karto::Size2::Size2
Size2(T width, T height)
Definition: Karto.h:889
karto::LocalizedRangeScan::m_PointReadings
PointVectorDouble m_PointReadings
Definition: Karto.h:5858
karto::Dataset::RemoveData
void RemoveData(LocalizedRangeScan *scan)
Definition: Karto.h:6650
karto::ParameterEnum::EnumMap
std::map< std::string, kt_int32s > EnumMap
Definition: Karto.h:3413
karto::GridIndexLookup
Definition: Karto.h:6890
karto::Name::operator=
Name & operator=(const Name &rOther)
Definition: Karto.h:491
karto::Vector2::SetY
void SetY(const T &y)
Definition: Karto.h:1055
karto::Rectangle2::SetPosition
void SetPosition(const T &rX, const T &rY)
Definition: Karto.h:1953
karto::Matrix3::operator*
Matrix3 operator*(const Matrix3 &rOther) const
Definition: Karto.h:2652
karto::Name::m_Name
std::string m_Name
Definition: Karto.h:613
karto::Rectangle2::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:2030
karto::SensorManager::GetAllSensors
SensorVector GetAllSensors()
Definition: Karto.h:3800
karto::Vector3::operator<<
friend std::ostream & operator<<(std::ostream &rStream, const Vector3 &rVector)
Definition: Karto.h:1534
karto::LaserRangeFinder::GetIs360Laser
kt_bool GetIs360Laser() const
Definition: Karto.h:4135
karto::Drive::Validate
virtual kt_bool Validate()
Definition: Karto.h:3876
karto::LaserRangeFinder::GetAngularResolution
kt_double GetAngularResolution() const
Definition: Karto.h:4049
karto::Vector3::Vector3
Vector3()
Definition: Karto.h:1299
karto::Quaternion::GetY
kt_double GetY() const
Definition: Karto.h:1642
karto::Singleton::m_pPointer
T * m_pPointer
Definition: Karto.h:248
karto::SensorData::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:5264
karto::Pose2::operator>>
friend std::istream & operator>>(std::istream &rStream, const Pose2 &)
Definition: Karto.h:2237
karto::LaserRangeFinder_Custom
@ LaserRangeFinder_Custom
Definition: Karto.h:3086
karto::LaserRangeFinder::m_pRangeThreshold
Parameter< kt_double > * m_pRangeThreshold
Definition: Karto.h:4392
karto::Grid::m_Height
kt_int32s m_Height
Definition: Karto.h:5031
karto::CustomData::CustomData
CustomData()
Definition: Karto.h:5075
karto::SensorManager
Definition: Karto.h:3701
karto::Functor
Definition: Karto.h:266
process_constraints.filename
filename
Definition: process_constraints.py:114
karto::Matrix3::Matrix3
Matrix3()
Definition: Karto.h:2450
karto::GridIndexLookup::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:7099
karto::OccupancyGrid::Clone
OccupancyGrid * Clone() const
Definition: Karto.h:6081
forEach
#define forEach(listtype, list)
Definition: Macros.h:66
ObjectType_Header
const kt_objecttype ObjectType_Header
Definition: Karto.h:83
karto::Pose2
Definition: Karto.h:2046
karto::SensorData::SetTime
void SetTime(kt_double time)
Definition: Karto.h:5190
karto::LaserRangeFinder::CreateLaserRangeFinder
static LaserRangeFinder * CreateLaserRangeFinder(LaserRangeFinderType type, const Name &rName)
Definition: Karto.h:4187
karto::Vector3::MakeCeil
void MakeCeil(const Vector3 &rOther)
Definition: Karto.h:1400
karto::Rectangle2::GetSize
const Size2< T > & GetSize() const
Definition: Karto.h:1971
karto::Vector2::MakeFloor
void MakeFloor(const Vector2 &rOther)
Definition: Karto.h:1064
karto::ParameterVector
std::vector< AbstractParameter * > ParameterVector
Definition: Karto.h:279
karto::Matrix::GetRows
kt_int32u GetRows() const
Definition: Karto.h:2771
karto::Object
Definition: Karto.h:634
karto::Quaternion::operator!=
kt_bool operator!=(const Quaternion &rOther) const
Definition: Karto.h:1788
karto::LaserRangeFinderType
LaserRangeFinderType
Definition: Karto.h:3084
karto::DatasetInfo::GetAuthor
const std::string & GetAuthor() const
Definition: Karto.h:6495
karto::LaserRangeFinder::GetType
kt_int32s GetType()
Definition: Karto.h:4116
karto::Vector2::SquaredLength
kt_double SquaredLength() const
Definition: Karto.h:1084
karto::Quaternion::Quaternion
Quaternion(kt_double x, kt_double y, kt_double z, kt_double w)
Definition: Karto.h:1600
karto::Module::Process
virtual kt_bool Process(karto::Object *)
Definition: Karto.h:846
karto::DrivePose::SetOdometricPose
void SetOdometricPose(const Pose3 &rPose)
Definition: Karto.h:5479
karto::math::IsUpTo
kt_bool IsUpTo(const T &value, const T &maximum)
Definition: Math.h:147
karto::LookupArray::GetArrayPointer
kt_int32s * GetArrayPointer() const
Definition: Karto.h:6847
karto::Pose2::operator+
Pose2 operator+(const Pose2 &rOther) const
Definition: Karto.h:2218
karto::Dataset::m_pDatasetInfo
DatasetInfo * m_pDatasetInfo
Definition: Karto.h:6717
karto::Vector2::operator<<
friend std::ostream & operator<<(std::ostream &rStream, const Vector2 &rVector)
Definition: Karto.h:1252
karto::GridStates_Occupied
@ GridStates_Occupied
Definition: Karto.h:4447
karto::Parameters::Parameters
Parameters(const std::string &rName)
Definition: Karto.h:3567
karto::Singleton::Singleton
Singleton()
Definition: Karto.h:217
assert.h
karto::Matrix::m_Rows
kt_int32u m_Rows
Definition: Karto.h:2856
karto::Vector2::Length
kt_double Length() const
Definition: Karto.h:1093
karto::IsSensor
kt_bool IsSensor(Object *pObject)
Definition: Karto.h:746
karto::SensorData::AddCustomData
void AddCustomData(CustomData *pCustomData)
Definition: Karto.h:5208
karto::Drive
Definition: Karto.h:3853
karto::DatasetInfo
Definition: Karto.h:6462
karto::Rectangle2::operator=
Rectangle2 & operator=(const Rectangle2 &rOther)
Definition: Karto.h:1998
karto::DrivePose::~DrivePose
virtual ~DrivePose()
Definition: Karto.h:5461
karto::LocalizedRangeScan::~LocalizedRangeScan
virtual ~LocalizedRangeScan()
Definition: Karto.h:5528
kt_int8u
uint8_t kt_int8u
Definition: Types.h:46
karto::IsSensorData
kt_bool IsSensorData(Object *pObject)
Definition: Karto.h:756
karto::LaserRangeScan
Definition: Karto.h:5288
karto::DatasetInfo::GetDescription
const std::string & GetDescription() const
Definition: Karto.h:6503
karto::Vector3
Definition: Karto.h:1293
karto::SensorData::SensorData
SensorData()
Definition: Karto.h:5134
karto::Grid::WorldToGrid
Vector2< kt_int32s > WorldToGrid(const Vector2< kt_double > &rWorld, kt_bool flipY=false) const
Definition: Karto.h:4801
karto::LocalizedRangeScan::LocalizedRangeScan
LocalizedRangeScan(const Name &rSensorName, const RangeReadingsVector &rReadings)
Definition: Karto.h:5516
karto::Sensor
Definition: Karto.h:3600
karto::Pose3::operator=
Pose3 & operator=(const Pose3 &rOther)
Definition: Karto.h:2387
karto::DatasetInfo::m_pTitle
Parameter< std::string > * m_pTitle
Definition: Karto.h:6525
karto::GridStates_Unknown
@ GridStates_Unknown
Definition: Karto.h:4446
karto::Name::operator<
kt_bool operator<(const Name &rOther) const
Definition: Karto.h:521
karto::Pose3::operator>>
friend std::istream & operator>>(std::istream &rStream, const Pose3 &)
Definition: Karto.h:2426
karto::BoundingBox2
Definition: Karto.h:2869
karto::BoundingBox2::GetMaximum
const Vector2< kt_double > & GetMaximum() const
Definition: Karto.h:2901
karto::Grid::GridToWorld
Vector2< kt_double > GridToWorld(const Vector2< kt_int32s > &rGrid, kt_bool flipY=false) const
Definition: Karto.h:4812
karto::GridIndexLookup::~GridIndexLookup
virtual ~GridIndexLookup()
Definition: Karto.h:6911
karto::INVALID_SCAN
const kt_int32s INVALID_SCAN
Definition: Math.h:47
karto::operator<<
std::ostream & operator<<(std::ostream &rStream, Exception &rException)
Definition: Karto.cpp:151
karto::LaserRangeScan::LaserRangeScan
LaserRangeScan()
Definition: Karto.h:5307
karto::AbstractParameter::SetValueFromString
virtual void SetValueFromString(const std::string &rStringValue)=0
karto::ObjectVector
std::vector< Object * > ObjectVector
Definition: Karto.h:734
karto::LocalizedRangeScan::GetCorrectedAt
Pose2 GetCorrectedAt(const Pose2 &sPose) const
Computes the pose of the robot if the sensor were at the given pose.
Definition: Karto.h:5670
karto::Vector2::Vector2
Vector2(T x, T y)
Definition: Karto.h:1017
karto::BoundingBox2::m_Maximum
Vector2< kt_double > m_Maximum
Definition: Karto.h:2966
karto::Vector2::operator*
kt_double operator*(const Vector2 &rOther) const
Definition: Karto.h:1181
karto::Parameter::GetValue
const T & GetValue() const
Definition: Karto.h:3273
karto::Name::Name
Name()
Definition: Karto.h:395
karto::Rectangle2::GetY
T GetY() const
Definition: Karto.h:1889
karto::LaserRangeFinder::SetMinimumAngle
void SetMinimumAngle(kt_double minimumAngle)
Definition: Karto.h:4018
karto::Pose2::SetX
void SetX(kt_double x)
Definition: Karto.h:2108
karto::Rectangle2::Rectangle2
Rectangle2()
Definition: Karto.h:1829
karto::ParameterManager
Definition: Karto.h:289
karto::Grid::GetDataPointer
T * GetDataPointer() const
Definition: Karto.h:4888
karto::ParameterEnum::m_EnumDefines
EnumMap m_EnumDefines
Definition: Karto.h:3534
karto::Quaternion::GetW
kt_double GetW() const
Definition: Karto.h:1678
karto::Grid::m_pCoordinateConverter
CoordinateConverter * m_pCoordinateConverter
Definition: Karto.h:5036
karto::Grid::GetBoundingBox
BoundingBox2 GetBoundingBox() const
Definition: Karto.h:4935
karto::Vector2::SquaredDistance
kt_double SquaredDistance(const Vector2 &rOther) const
Definition: Karto.h:1102
karto::RangeReadingsVector
std::vector< kt_double > RangeReadingsVector
Definition: Karto.h:5283
karto::ParameterManager::m_ParameterLookup
std::map< std::string, AbstractParameter * > m_ParameterLookup
Definition: Karto.h:366
karto::Pose2::GetHeading
kt_double GetHeading() const
Definition: Karto.h:2153
ObjectType_LaserRangeScan
const kt_objecttype ObjectType_LaserRangeScan
Definition: Karto.h:78
karto::LaserRangeScan::GetRangeReadingsVector
RangeReadingsVector GetRangeReadingsVector() const
Definition: Karto.h:5345
karto::Sensor::Sensor
Sensor()
Definition: Karto.h:3607
karto::Rectangle2::operator==
kt_bool operator==(const Rectangle2 &rOther) const
Definition: Karto.h:2009
karto::DrivePose::GetOdometricPose
const Pose3 & GetOdometricPose() const
Definition: Karto.h:5470
karto::OccupancyGrid::m_pCellPassCnt
Grid< kt_int32u > * m_pCellPassCnt
Definition: Karto.h:6421
karto::Object::GetParameter
AbstractParameter * GetParameter(const std::string &rName) const
Definition: Karto.h:689
karto::Rectangle2::m_Position
Vector2< T > m_Position
Definition: Karto.h:2023
karto::SensorData::GetTime
kt_double GetTime() const
Definition: Karto.h:5181
karto::BoundingBox2::m_Minimum
Vector2< kt_double > m_Minimum
Definition: Karto.h:2965
karto::OccupancyGrid::Update
virtual void Update()
Definition: Karto.h:6386
karto::LocalizedRangeScanMap
std::map< int, LocalizedRangeScan * > LocalizedRangeScanMap
Definition: Karto.h:5880
karto::Transform::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3069
karto::Rectangle2::SetSize
void SetSize(const Size2< T > &rSize)
Definition: Karto.h:1980
karto::DatasetInfo::m_pAuthor
Parameter< std::string > * m_pAuthor
Definition: Karto.h:6526
karto::AbstractParameter::access
friend class boost::serialization::access
Definition: Karto.h:3209
karto::Rectangle2::Rectangle2
Rectangle2(const Vector2< T > &rPosition, const Size2< T > &rSize)
Definition: Karto.h:1851
karto
Definition: Karto.h:88
karto::DatasetInfo::~DatasetInfo
virtual ~DatasetInfo()
Definition: Karto.h:6479
karto::Size2::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:985
karto::LaserRangeScan::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:5421
karto::Pose2::operator+=
void operator+=(const Pose2 &rOther)
Definition: Karto.h:2207
karto::LaserRangeScan::~LaserRangeScan
virtual ~LaserRangeScan()
Definition: Karto.h:5329
karto::Vector3::operator*
const Vector3 operator*(T scalar) const
Definition: Karto.h:1502
karto::Name::IsValid
kt_bool IsValid(char c)
Definition: Karto.h:607
karto::Grid::GetHeight
kt_int32s GetHeight() const
Definition: Karto.h:4852
karto::Module::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:857
karto::SensorData::GetSensorName
const Name & GetSensorName() const
Definition: Karto.h:5199
karto::Pose3::ToString
std::string ToString()
Definition: Karto.h:2373
karto::Exception::m_Message
std::string m_Message
Definition: Karto.h:169
karto::Parameter::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Karto.h:3344


slam_toolbox
Author(s): Steve Macenski
autogenerated on Thu Jan 11 2024 03:37:55