Mapper.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_MAPPER_H
19 #define karto_sdk_MAPPER_H
20 
21 #include <map>
22 #include <vector>
23 #include <unordered_map>
24 #include <queue>
25 
26 #include <Eigen/Core>
27 
28 #include "tbb/parallel_for.h"
29 #include "tbb/blocked_range.h"
30 #include "tbb/tbb.h"
31 #include <algorithm>
32 #include <chrono>
33 
34 #include <karto_sdk/Karto.h>
35 
36 #include "nanoflann_adaptors.h"
37 
38 namespace karto
39 {
41  // Listener classes
42 
47  {
48  public:
52  virtual void Info(const std::string& /*rInfo*/) {};
53  friend class boost::serialization::access;
54  template<class Archive>
55  void serialize(Archive &ar, const unsigned int version)
56  {
57  }
58  };
59  BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperListener)
64  {
65  public:
69  virtual void Debug(const std::string& /*rInfo*/) {};
70  friend class boost::serialization::access;
71  template<class Archive>
72  void serialize(Archive &ar, const unsigned int version)
73  {
74  }
75  };
76  BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperDebugListener)
81  {
82  public:
86  virtual void LoopClosureCheck(const std::string& /*rInfo*/) {};
87 
91  virtual void BeginLoopClosure(const std::string& /*rInfo*/) {};
92 
96  virtual void EndLoopClosure(const std::string& /*rInfo*/) {};
97  friend class boost::serialization::access;
98  template<class Archive>
99  void serialize(Archive &ar, const unsigned int version)
100  {
101  }
102  }; // MapperLoopClosureListener
103  BOOST_SERIALIZATION_ASSUME_ABSTRACT(MapperLoopClosureListener)
107 
111  class EdgeLabel
112  {
113  public:
118  {
119  }
120 
124  virtual ~EdgeLabel()
125  {
126  }
127  friend class boost::serialization::access;
128  template<class Archive>
129  void serialize(Archive &ar, const unsigned int version)
130  {
131  }
132  }; // EdgeLabel
133 
137 
138  // A LinkInfo object contains the requisite information for the "spring"
139  // that links two scans together--the pose difference and the uncertainty
140  // (represented by a covariance matrix).
141  class LinkInfo : public EdgeLabel
142  {
143  public:
151  {
152  }
153  LinkInfo(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
154  {
155  Update(rPose1, rPose2, rCovariance);
156  }
157 
161  virtual ~LinkInfo()
162  {
163  }
164 
165  public:
172  void Update(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
173  {
174  m_Pose1 = rPose1;
175  m_Pose2 = rPose2;
176 
177  // transform second pose into the coordinate system of the first pose
178  Transform transform(rPose1, Pose2());
179  m_PoseDifference = transform.TransformPose(rPose2);
180 
181  // transform covariance into reference of first pose
182  Matrix3 rotationMatrix;
183  rotationMatrix.FromAxisAngle(0, 0, 1, -rPose1.GetHeading());
184 
185  m_Covariance = rotationMatrix * rCovariance * rotationMatrix.Transpose();
186  }
187 
192  inline const Pose2& GetPose1()
193  {
194  return m_Pose1;
195  }
196 
201  inline const Pose2& GetPose2()
202  {
203  return m_Pose2;
204  }
205 
210  inline const Pose2& GetPoseDifference()
211  {
212  return m_PoseDifference;
213  }
214 
219  inline const Matrix3& GetCovariance()
220  {
221  return m_Covariance;
222  }
223 
224  private:
229 
230  friend class boost::serialization::access;
231  template<class Archive>
232  void serialize(Archive &ar, const unsigned int version)
233  {
234  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(EdgeLabel);
235  ar & BOOST_SERIALIZATION_NVP(m_Pose1);
236  ar & BOOST_SERIALIZATION_NVP(m_Pose2);
237  ar & BOOST_SERIALIZATION_NVP(m_PoseDifference);
238  ar & BOOST_SERIALIZATION_NVP(m_Covariance);
239  }
240  }; // LinkInfo
241 
245 
246  template<typename T>
247  class Edge;
248 
252  template<typename T>
253  class Vertex
254  {
255  friend class Edge<T>;
256 
257  public:
263  : m_pObject(NULL), m_Score(1.0)
264  {
265  }
266  Vertex(T* pObject)
267  : m_pObject(pObject), m_Score(1.0)
268  {
269  }
270 
274  virtual ~Vertex()
275  {
276  }
277 
282  inline const std::vector<Edge<T>*>& GetEdges() const
283  {
284  return m_Edges;
285  }
286 
290  inline void RemoveEdge(const int& idx)
291  {
292  m_Edges[idx] = NULL;
293  m_Edges.erase(m_Edges.begin() + idx);
294  return;
295  }
296 
301  inline const double GetScore() const
302  {
303  return m_Score;
304  }
305 
310  inline void SetScore(const double score)
311  {
312  m_Score = score;
313  }
314 
319  inline T* GetObject() const
320  {
321  return m_pObject;
322  }
323 
327  inline void RemoveObject()
328  {
329  m_pObject = NULL;
330  }
331 
336  std::vector<Vertex<T>*> GetAdjacentVertices() const
337  {
338  std::vector<Vertex<T>*> vertices;
339 
340  const_forEach(typename std::vector<Edge<T>*>, &m_Edges)
341  {
342  Edge<T>* pEdge = *iter;
343 
344  if (pEdge == NULL)
345  {
346  continue;
347  }
348 
349  // check both source and target because we have a undirected graph
350  if (pEdge->GetSource() != this)
351  {
352  vertices.push_back(pEdge->GetSource());
353  }
354 
355  if (pEdge->GetTarget() != this)
356  {
357  vertices.push_back(pEdge->GetTarget());
358  }
359  }
360 
361  return vertices;
362  }
363 
364  private:
369  inline void AddEdge(Edge<T>* pEdge)
370  {
371  m_Edges.push_back(pEdge);
372  }
373 
375  std::vector<Edge<T>*> m_Edges;
377 
378  friend class boost::serialization::access;
379  template<class Archive>
380  void serialize(Archive &ar, const unsigned int version)
381  {
382  ar & BOOST_SERIALIZATION_NVP(m_pObject);
383  ar & BOOST_SERIALIZATION_NVP(m_Edges);
384  ar & BOOST_SERIALIZATION_NVP(m_Score);
385  }
386  }; // Vertex<T>
387 
391 
395  template<typename T>
396  class Edge
397  {
398  public:
405  : m_pSource(NULL)
406  , m_pTarget(NULL)
407  , m_pLabel(NULL)
408  {
409  }
410  Edge(Vertex<T>* pSource, Vertex<T>* pTarget)
411  : m_pSource(pSource)
412  , m_pTarget(pTarget)
413  , m_pLabel(NULL)
414  {
415  m_pSource->AddEdge(this);
416  m_pTarget->AddEdge(this);
417  }
418 
422  virtual ~Edge()
423  {
424  m_pSource = NULL;
425  m_pTarget = NULL;
426 
427  if (m_pLabel != NULL)
428  {
429  delete m_pLabel;
430  m_pLabel = NULL;
431  }
432  }
433 
434  public:
439  inline Vertex<T>* GetSource() const
440  {
441  return m_pSource;
442  }
443 
448  inline Vertex<T>* GetTarget() const
449  {
450  return m_pTarget;
451  }
452 
457  inline EdgeLabel* GetLabel()
458  {
459  return m_pLabel;
460  }
461 
466  inline void SetLabel(EdgeLabel* pLabel)
467  {
468  m_pLabel = pLabel;
469  }
470 
471  private:
475 
476  friend class boost::serialization::access;
477  template<class Archive>
478  void serialize(Archive &ar, const unsigned int version)
479  {
480  ar & BOOST_SERIALIZATION_NVP(m_pSource);
481  ar & BOOST_SERIALIZATION_NVP(m_pTarget);
482  ar & BOOST_SERIALIZATION_NVP(m_pLabel);
483  }
484  }; // class Edge<T>
485 
489 
493  template<typename T>
494  class Visitor
495  {
496  public:
502  virtual kt_bool Visit(Vertex<T>* pVertex) = 0;
503  friend class boost::serialization::access;
504  template<class Archive>
505  void serialize(Archive &ar, const unsigned int version)
506  {
507  }
508  }; // Visitor<T>
509  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Visitor<LocalizedRangeScan>)
513 
514  template<typename T>
515  class Graph;
516 
520  template<typename T>
522  {
523  public:
525  {
526  }
528  : m_pGraph(pGraph)
529  {
530  }
531 
532  virtual ~GraphTraversal()
533  {
534  }
535 
536  public:
537 
538  virtual std::vector<T*> TraverseForScans(Vertex<T>* pStartVertex, Visitor<T>* pVisitor) = 0;
539  virtual std::vector<Vertex<T>*> TraverseForVertices(Vertex<T>* pStartVertex, Visitor<T>* pVisitor) = 0;
540 
541  protected:
543 
544  friend class boost::serialization::access;
545  template<class Archive>
546  void serialize(Archive &ar, const unsigned int version)
547  {
548  ar & BOOST_SERIALIZATION_NVP(m_pGraph);
549  }
550  }; // GraphTraversal<T>
551  BOOST_SERIALIZATION_ASSUME_ABSTRACT(GraphTraversal<LocalizedRangeScan>)
552 
553 
554 
560  template<typename T>
561  class Graph
562  {
563  public:
567  typedef std::map<Name, std::map<int, Vertex<T>*> > VertexMap;
568 
569  public:
574  {
575  }
576 
580  virtual ~Graph()
581  {
582  Clear();
583  }
584 
585  public:
591  inline void AddVertex(const Name& rName, Vertex<T>* pVertex)
592  {
593  m_Vertices[rName].insert({pVertex->GetObject()->GetStateId(), pVertex});
594  }
595 
601  inline void RemoveVertex(const Name& rName, const int& idx)
602  {
603  std::map<int, Vertex<LocalizedRangeScan>* >::iterator it = m_Vertices[rName].find(idx);
604  if (it != m_Vertices[rName].end())
605  {
606  it->second = NULL;
607  m_Vertices[rName].erase(it);
608  }
609  else
610  {
611  std::cout << "RemoveVertex: Failed to remove vertex " << idx
612  << " because it doesnt exist in m_Vertices." << std::endl;
613  }
614  }
615 
620  inline void AddEdge(Edge<T>* pEdge)
621  {
622  m_Edges.push_back(pEdge);
623  }
624 
629  inline void RemoveEdge(const int& idx)
630  {
631  m_Edges[idx] = NULL;
632  m_Edges.erase(m_Edges.begin() + idx);
633  }
634 
635 
639  void Clear()
640  {
641  forEachAs(typename VertexMap, &m_Vertices, indexIter)
642  {
643  // delete each vertex
644  typename std::map<int, Vertex<T>*>::iterator iter;
645  for (iter = indexIter->second.begin(); iter != indexIter->second.end(); ++iter)
646  {
647  delete iter->second;
648  iter->second = nullptr;
649  }
650  }
651  m_Vertices.clear();
652 
653  forEach(typename std::vector<Edge<T>*>, &m_Edges)
654  {
655  delete *iter;
656  *iter = nullptr;
657  }
658  m_Edges.clear();
659  }
660 
665  inline const std::vector<Edge<T>*>& GetEdges() const
666  {
667  return m_Edges;
668  }
669 
674  inline const VertexMap& GetVertices() const
675  {
676  return m_Vertices;
677  }
678 
679  protected:
683  VertexMap m_Vertices;
684 
688  std::vector<Edge<T>*> m_Edges;
692  friend class boost::serialization::access;
693  template<class Archive>
694  void serialize(Archive &ar, const unsigned int version)
695  {
696  std::cout << "Graph <- m_Edges; ";
697  ar & BOOST_SERIALIZATION_NVP(m_Edges);
698  std::cout << "Graph <- m_Vertices\n";
699  ar & BOOST_SERIALIZATION_NVP(m_Vertices);
700  }
701  }; // Graph<T>
702 
706 
707  class Mapper;
708  class ScanMatcher;
709 
713  class KARTO_EXPORT MapperGraph : public Graph<LocalizedRangeScan>
714  {
715  public:
721  MapperGraph(Mapper* pMapper, kt_double rangeThreshold);
723  {
724  }
728  virtual ~MapperGraph();
729 
730  public:
736 
745  Edge<LocalizedRangeScan>* AddEdge(LocalizedRangeScan* pSourceScan,
746  LocalizedRangeScan* pTargetScan,
747  kt_bool& rIsNewEdge);
748 
754  void AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance);
755 
761  kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName);
762 
766  void CorrectPoses();
767 
773  LocalizedRangeScanVector FindNearLinkedScans(LocalizedRangeScan* pScan, kt_double maxDistance);
774 
780  std::vector<Vertex<LocalizedRangeScan>*> FindNearLinkedVertices(LocalizedRangeScan* pScan, kt_double maxDistance);
781 
787  LocalizedRangeScanVector FindNearByScans(Name name, const Pose2 refPose, kt_double maxDistance);
788 
794  std::vector<Vertex<LocalizedRangeScan>*> FindNearByVertices(Name name, const Pose2 refPose, kt_double maxDistance);
795 
800  Vertex<LocalizedRangeScan>* FindNearByScan(Name name, const Pose2 refPose);
801 
807  {
808  return m_pLoopScanMatcher;
809  }
810 
811  private:
818  {
819  Name rName = pScan->GetSensorName();
820  std::map<int, Vertex<LocalizedRangeScan>* >::iterator it = m_Vertices[rName].find(pScan->GetStateId());
821  if (it != m_Vertices[rName].end())
822  {
823  return it->second;
824  }
825  else
826  {
827  std::cout << "GetVertex: Failed to get vertex, idx " << pScan->GetStateId() <<
828  " is not in m_Vertices." << std::endl;
829  return nullptr;
830  }
831  }
832 
838  LocalizedRangeScan* GetClosestScanToPose(const LocalizedRangeScanVector& rScans, const Pose2& rPose) const;
839 
847  void LinkScans(LocalizedRangeScan* pFromScan,
848  LocalizedRangeScan* pToScan,
849  const Pose2& rMean,
850  const Matrix3& rCovariance);
851 
858  void LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector<Matrix3>& rCovariances);
859 
867  void LinkChainToScan(const LocalizedRangeScanVector& rChain,
868  LocalizedRangeScan* pScan,
869  const Pose2& rMean,
870  const Matrix3& rCovariance);
871 
877  std::vector<LocalizedRangeScanVector> FindNearChains(LocalizedRangeScan* pScan);
878 
885  Pose2 ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector<Matrix3>& rCovariances) const;
886 
895  LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan* pScan,
896  const Name& rSensorName,
897  kt_int32u& rStartNum);
898 
899  private:
904 
909 
914 
918  friend class boost::serialization::access;
919  template<class Archive>
920  void serialize(Archive &ar, const unsigned int version)
921  {
922  std::cout << "MapperGraph <- Graph; ";
923  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Graph<LocalizedRangeScan>);
924  std::cout << "MapperGraph <- m_pMapper; ";
925  ar & BOOST_SERIALIZATION_NVP(m_pMapper);
926  std::cout << "MapperGraph <- m_pLoopScanMatcher; ";
927  ar & BOOST_SERIALIZATION_NVP(m_pLoopScanMatcher);
928  std::cout << "MapperGraph <- m_pTraversal\n";
929  ar & BOOST_SERIALIZATION_NVP(m_pTraversal);
930  }
931 
932  }; // MapperGraph
933 
937 
942  {
943  public:
947  typedef std::vector<std::pair<kt_int32s, Pose2> > IdPoseVector;
948 
953  {
954  }
955 
959  virtual ~ScanSolver()
960  {
961  }
962 
963  public:
967  virtual void Compute() = 0;
968 
973  virtual const IdPoseVector& GetCorrections() const = 0;
974 
978  virtual void AddNode(Vertex<LocalizedRangeScan>* /*pVertex*/)
979  {
980  }
981 
985  virtual void RemoveNode(kt_int32s /*id*/)
986  {
987  }
988 
992  virtual void AddConstraint(Edge<LocalizedRangeScan>* /*pEdge*/)
993  {
994  }
995 
999  virtual void RemoveConstraint(kt_int32s /*sourceId*/, kt_int32s /*targetId*/)
1000  {
1001  }
1002 
1006  virtual void Clear()
1007  {
1008  }
1009 
1013  virtual void Reset()
1014  {
1015  }
1016 
1020  virtual std::unordered_map<int, Eigen::Vector3d>* getGraph()
1021  {
1022  std::cout << "getGraph method not implemented for this solver type. Graph visualization unavailable." << std::endl;
1023  }
1024 
1028  virtual void ModifyNode(const int& unique_id, Eigen::Vector3d pose)
1029  {
1030  std::cout << "ModifyNode method not implemented for this solver type. Manual loop closure unavailable." << std::endl;
1031  };
1035  virtual void GetNodeOrientation(const int& unique_id, double& pose)
1036  {
1037  std::cout << "GetNodeOrientation method not implemented for this solver type." << std::endl;
1038  };
1039 
1040  friend class boost::serialization::access;
1041  template<class Archive>
1042  void serialize(Archive &ar, const unsigned int version)
1043  {
1044  }
1045  }; // ScanSolver
1046  BOOST_SERIALIZATION_ASSUME_ABSTRACT(ScanSolver)
1050 
1055  {
1056  public:
1061  {
1062  if (m_pKernel)
1063  {
1064  delete [] m_pKernel;
1065  }
1066 
1067  }
1068 
1069  public:
1079  {
1080  }
1082  kt_int32s height,
1083  kt_double resolution,
1084  kt_double smearDeviation)
1085  {
1086  assert(resolution != 0.0);
1087 
1088  // +1 in case of roundoff
1089  kt_int32u borderSize = GetHalfKernelSize(smearDeviation, resolution) + 1;
1090 
1091  CorrelationGrid* pGrid = new CorrelationGrid(width, height, borderSize, resolution, smearDeviation);
1092 
1093  return pGrid;
1094  }
1095 
1102  virtual kt_int32s GridIndex(const Vector2<kt_int32s>& rGrid, kt_bool boundaryCheck = true) const
1103  {
1104  kt_int32s x = rGrid.GetX() + m_Roi.GetX();
1105  kt_int32s y = rGrid.GetY() + m_Roi.GetY();
1106 
1107  return Grid<kt_int8u>::GridIndex(Vector2<kt_int32s>(x, y), boundaryCheck);
1108  }
1109 
1114  inline const Rectangle2<kt_int32s>& GetROI() const
1115  {
1116  return m_Roi;
1117  }
1118 
1123  inline void SetROI(const Rectangle2<kt_int32s>& roi)
1124  {
1125  m_Roi = roi;
1126  }
1127 
1132  inline void SmearPoint(const Vector2<kt_int32s>& rGridPoint)
1133  {
1134  assert(m_pKernel != NULL);
1135 
1136  int gridIndex = GridIndex(rGridPoint);
1137  if (GetDataPointer()[gridIndex] != GridStates_Occupied)
1138  {
1139  return;
1140  }
1141 
1142  kt_int32s halfKernel = m_KernelSize / 2;
1143 
1144  // apply kernel
1145  for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
1146  {
1147  kt_int8u* pGridAdr = GetDataPointer(Vector2<kt_int32s>(rGridPoint.GetX(), rGridPoint.GetY() + j));
1148 
1149  kt_int32s kernelConstant = (halfKernel) + m_KernelSize * (j + halfKernel);
1150 
1151  // if a point is on the edge of the grid, there is no problem
1152  // with running over the edge of allowable memory, because
1153  // the grid has margins to compensate for the kernel size
1154  for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
1155  {
1156  kt_int32s kernelArrayIndex = i + kernelConstant;
1157 
1158  kt_int8u kernelValue = m_pKernel[kernelArrayIndex];
1159  if (kernelValue > pGridAdr[i])
1160  {
1161  // kernel value is greater, so set it to kernel value
1162  pGridAdr[i] = kernelValue;
1163  }
1164  }
1165  }
1166  }
1167 
1168  protected:
1177  CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize,
1178  kt_double resolution, kt_double smearDeviation)
1179  : Grid<kt_int8u>(width + borderSize * 2, height + borderSize * 2)
1180  , m_SmearDeviation(smearDeviation)
1181  , m_pKernel(NULL)
1182  {
1183  GetCoordinateConverter()->SetScale(1.0 / resolution);
1184 
1185  // setup region of interest
1186  m_Roi = Rectangle2<kt_int32s>(borderSize, borderSize, width, height);
1187 
1188  // calculate kernel
1189  CalculateKernel();
1190  }
1191 
1195  virtual void CalculateKernel()
1196  {
1197  kt_double resolution = GetResolution();
1198 
1199  assert(resolution != 0.0);
1200  assert(m_SmearDeviation != 0.0);
1201 
1202  // min and max distance deviation for smearing;
1203  // will smear for two standard deviations, so deviation must be at least 1/2 of the resolution
1204  const kt_double MIN_SMEAR_DISTANCE_DEVIATION = 0.5 * resolution;
1205  const kt_double MAX_SMEAR_DISTANCE_DEVIATION = 10 * resolution;
1206 
1207  // check if given value too small or too big
1208  if (!math::InRange(m_SmearDeviation, MIN_SMEAR_DISTANCE_DEVIATION, MAX_SMEAR_DISTANCE_DEVIATION))
1209  {
1210  std::stringstream error;
1211  error << "Mapper Error: Smear deviation too small: Must be between "
1212  << MIN_SMEAR_DISTANCE_DEVIATION
1213  << " and "
1214  << MAX_SMEAR_DISTANCE_DEVIATION;
1215  throw std::runtime_error(error.str());
1216  }
1217 
1218  // NOTE: Currently assumes a two-dimensional kernel
1219 
1220  // +1 for center
1221  m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1;
1222 
1223  // allocate kernel
1224  m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];
1225  if (m_pKernel == NULL)
1226  {
1227  throw std::runtime_error("Unable to allocate memory for kernel!");
1228  }
1229 
1230  // calculate kernel
1231  kt_int32s halfKernel = m_KernelSize / 2;
1232  for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
1233  {
1234  for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
1235  {
1236 #ifdef WIN32
1237  kt_double distanceFromMean = _hypot(i * resolution, j * resolution);
1238 #else
1239  kt_double distanceFromMean = hypot(i * resolution, j * resolution);
1240 #endif
1241  kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2));
1242 
1243  kt_int32u kernelValue = static_cast<kt_int32u>(math::Round(z * GridStates_Occupied));
1244  assert(math::IsUpTo(kernelValue, static_cast<kt_int32u>(255)));
1245 
1246  int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel);
1247  m_pKernel[kernelArrayIndex] = static_cast<kt_int8u>(kernelValue);
1248  }
1249  }
1250  }
1251 
1259  static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution)
1260  {
1261  assert(resolution != 0.0);
1262 
1263  return static_cast<kt_int32s>(math::Round(2.0 * smearDeviation / resolution));
1264  }
1265 
1266  private:
1272 
1273  // Size of one side of the kernel
1275 
1276  // Cached kernel for smearing
1278 
1279  // region of interest
1284  friend class boost::serialization::access;
1285  template<class Archive>
1286  void serialize(Archive &ar, const unsigned int version)
1287  {
1288  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Grid<kt_int8u>);
1289  ar & BOOST_SERIALIZATION_NVP(m_SmearDeviation);
1290  ar & BOOST_SERIALIZATION_NVP(m_KernelSize);
1291  if (Archive::is_loading::value)
1292  {
1293  m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];
1294  }
1295  ar & boost::serialization::make_array<kt_int8u>(m_pKernel, m_KernelSize * m_KernelSize);
1296  ar & BOOST_SERIALIZATION_NVP(m_Roi);
1297  }
1298  }; // CorrelationGrid
1299  BOOST_SERIALIZATION_ASSUME_ABSTRACT(CorrelationGrid)
1303 
1308  {
1309  public:
1311  {
1312  }
1316  virtual ~ScanMatcher();
1317 
1318  public:
1322  void operator() (const kt_double& y) const;
1323 
1327  static ScanMatcher* Create(Mapper* pMapper,
1328  kt_double searchSize,
1329  kt_double resolution,
1330  kt_double smearDeviation,
1331  kt_double rangeThreshold);
1332 
1343  template<class T = LocalizedRangeScanVector>
1344  kt_double MatchScan(LocalizedRangeScan* pScan,
1345  const T& rBaseScans,
1346  Pose2& rMean, Matrix3& rCovariance,
1347  kt_bool doPenalize = true,
1348  kt_bool doRefineMatch = true);
1349 
1366  kt_double CorrelateScan(LocalizedRangeScan* pScan,
1367  const Pose2& rSearchCenter,
1368  const Vector2<kt_double>& rSearchSpaceOffset,
1369  const Vector2<kt_double>& rSearchSpaceResolution,
1370  kt_double searchAngleOffset,
1371  kt_double searchAngleResolution,
1372  kt_bool doPenalize,
1373  Pose2& rMean,
1374  Matrix3& rCovariance,
1375  kt_bool doingFineMatch);
1376 
1387  void ComputePositionalCovariance(const Pose2& rBestPose,
1388  kt_double bestResponse,
1389  const Pose2& rSearchCenter,
1390  const Vector2<kt_double>& rSearchSpaceOffset,
1391  const Vector2<kt_double>& rSearchSpaceResolution,
1392  kt_double searchAngleResolution,
1393  Matrix3& rCovariance);
1394 
1404  void ComputeAngularCovariance(const Pose2& rBestPose,
1405  kt_double bestResponse,
1406  const Pose2& rSearchCenter,
1407  kt_double searchAngleOffset,
1408  kt_double searchAngleResolution,
1409  Matrix3& rCovariance);
1410 
1416  {
1417  return m_pCorrelationGrid;
1418  }
1419 
1420  private:
1426  void AddScans(const LocalizedRangeScanVector& rScans, Vector2<kt_double> viewPoint);
1427  void AddScans(const LocalizedRangeScanMap& rScans, Vector2<kt_double> viewPoint);
1428 
1435  void AddScan(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint, kt_bool doSmear = true);
1436 
1443  PointVectorDouble FindValidPoints(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint) const;
1444 
1451  kt_double GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const;
1452 
1453  protected:
1458  : m_pMapper(pMapper)
1459  , m_pCorrelationGrid(NULL)
1460  , m_pSearchSpaceProbs(NULL)
1461  , m_pGridLookup(NULL)
1462  , m_doPenalize(false)
1463  {
1464  }
1465 
1466  private:
1471  std::pair<kt_double, Pose2>* m_pPoseResponse;
1472  std::vector<kt_double> m_xPoses;
1473  std::vector<kt_double> m_yPoses;
1479 
1483  friend class boost::serialization::access;
1484  template<class Archive>
1485  void serialize(Archive &ar, const unsigned int version)
1486  {
1487  ar & BOOST_SERIALIZATION_NVP(m_pMapper);
1488  ar & BOOST_SERIALIZATION_NVP(m_pCorrelationGrid);
1489  ar & BOOST_SERIALIZATION_NVP(m_pSearchSpaceProbs);
1490  ar & BOOST_SERIALIZATION_NVP(m_pGridLookup);
1491  ar & BOOST_SERIALIZATION_NVP(m_xPoses);
1492  ar & BOOST_SERIALIZATION_NVP(m_yPoses);
1493  ar & BOOST_SERIALIZATION_NVP(m_rSearchCenter);
1494  ar & BOOST_SERIALIZATION_NVP(m_searchAngleResolution);
1495  ar & BOOST_SERIALIZATION_NVP(m_nAngles);
1496  ar & BOOST_SERIALIZATION_NVP(m_searchAngleResolution);;
1497  ar & BOOST_SERIALIZATION_NVP(m_doPenalize);
1498  kt_int32u poseResponseSize = static_cast<kt_int32u>(m_xPoses.size() * m_yPoses.size() * m_nAngles);
1499  if (Archive::is_loading::value)
1500  {
1501  m_pPoseResponse = new std::pair<kt_double, Pose2>[poseResponseSize];
1502  }
1503  ar & boost::serialization::make_array<std::pair<kt_double, Pose2>>(m_pPoseResponse, poseResponseSize);
1504  }
1505 
1506  }; // ScanMatcher
1507 
1511 
1512  class ScanManager;
1513 
1517  class KARTO_EXPORT MapperSensorManager //: public SensorManager // was commented out, works with it in, but I'll leave out for now...
1518  {
1519  typedef std::map<Name, ScanManager*> ScanManagerMap;
1520 
1521  public:
1525  MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
1526  : m_RunningBufferMaximumSize(runningBufferMaximumSize)
1527  , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
1528  , m_NextScanId(0)
1529  {
1530  }
1531 
1533  }
1534 
1539  {
1540  Clear();
1541  }
1542 
1543  public:
1549  void RegisterSensor(const Name& rSensorName);
1550 
1557  LocalizedRangeScan* GetScan(const Name& rSensorName, kt_int32s scanIndex);
1558 
1563  inline std::vector<Name> GetSensorNames()
1564  {
1565  std::vector<Name> deviceNames;
1566  const_forEach(ScanManagerMap, &m_ScanManagers)
1567  {
1568  deviceNames.push_back(iter->first);
1569  }
1570 
1571  return deviceNames;
1572  }
1573 
1579  LocalizedRangeScan* GetLastScan(const Name& rSensorName);
1580 
1585  void SetLastScan(LocalizedRangeScan* pScan);
1586 
1593  {
1594  std::map<int, LocalizedRangeScan*>::iterator it = m_Scans.find(id);
1595  if (it != m_Scans.end())
1596  {
1597  return it->second;
1598  }
1599  else
1600  {
1601  std::cout << "GetScan: id " << id <<
1602  " does not exist in m_scans, cannot retrieve it." << std::endl;
1603  return nullptr;
1604  }
1605  }
1606 
1611  void AddScan(LocalizedRangeScan* pScan);
1612 
1617  void AddRunningScan(LocalizedRangeScan* pScan);
1618 
1623  void RemoveScan(LocalizedRangeScan* pScan);
1624 
1630  LocalizedRangeScanMap& GetScans(const Name& rSensorName);
1631 
1637  LocalizedRangeScanVector& GetRunningScans(const Name& rSensorName);
1638 
1642  void ClearRunningScans(const Name& rSensorName);
1643 
1647  kt_int32u GetRunningScanBufferSize(const Name& rSensorName);
1648 
1653  LocalizedRangeScanVector GetAllScans();
1654 
1658  void Clear();
1659 
1660  private:
1666  {
1667  return GetScanManager(pScan->GetSensorName());
1668  }
1669 
1675  inline ScanManager* GetScanManager(const Name& rSensorName)
1676  {
1677  if (m_ScanManagers.find(rSensorName) != m_ScanManagers.end())
1678  {
1679  return m_ScanManagers[rSensorName];
1680  }
1681 
1682  return NULL;
1683  }
1684 
1685  friend class boost::serialization::access;
1686  template<class Archive>
1687  void serialize(Archive &ar, const unsigned int version)
1688  {
1689  std::cout << "MapperSensorManager <- m_ScanManagers; ";
1690  ar & BOOST_SERIALIZATION_NVP(m_ScanManagers);
1691  ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumSize);
1692  ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumDistance);
1693  ar & BOOST_SERIALIZATION_NVP(m_NextScanId);
1694  std::cout << "MapperSensorManager <- m_Scans\n";
1695  ar & BOOST_SERIALIZATION_NVP(m_Scans);
1696  }
1697 
1698  private:
1699  // map from device ID to scan data
1700  ScanManagerMap m_ScanManagers;
1701 
1704 
1706 
1707  std::map<int, LocalizedRangeScan*> m_Scans;
1708  }; // MapperSensorManager
1709 
1713 
1873  {
1875  LocalizationScanVertex(const LocalizationScanVertex& obj){scan = obj.scan; vertex = obj.vertex;};
1878  };
1879 
1880  typedef std::queue<LocalizationScanVertex> LocalizationScanVertices;
1881 
1882  class KARTO_EXPORT Mapper : public Module
1883  {
1884  friend class MapperGraph;
1885  friend class ScanMatcher;
1886 
1887  public:
1891  Mapper();
1892 
1897  Mapper(const std::string& rName);
1898 
1902  virtual ~Mapper();
1903 
1904  public:
1909  void Initialize(kt_double rangeThreshold);
1910 
1915  void SaveToFile(const std::string& filename);
1916 
1921  void LoadFromFile(const std::string& filename);
1922 
1927  virtual void Reset();
1928 
1940  virtual kt_bool Process(LocalizedRangeScan* pScan);
1941 
1945  virtual kt_bool Process(Object* pObject);
1946 
1947  // processors
1948  kt_bool ProcessAtDock(LocalizedRangeScan* pScan);
1949  kt_bool ProcessAgainstNode(LocalizedRangeScan* pScan, const int& nodeId);
1950  kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan* pScan);
1951  kt_bool ProcessLocalization(LocalizedRangeScan* pScan);
1952  kt_bool RemoveNodeFromGraph(Vertex<LocalizedRangeScan>*);
1953 
1960  virtual const LocalizedRangeScanVector GetAllProcessedScans() const;
1961 
1966  void AddListener(MapperListener* pListener);
1967 
1972  void RemoveListener(MapperListener* pListener);
1973 
1978  void SetScanSolver(ScanSolver* pSolver);
1979 
1984  ScanSolver* getScanSolver();
1985 
1990  virtual MapperGraph* GetGraph() const;
1991 
1996  virtual ScanMatcher* GetSequentialScanMatcher() const;
1997 
2002  virtual ScanMatcher* GetLoopScanMatcher() const;
2003 
2009  {
2010  return m_pMapperSensorManager;
2011  }
2012 
2018  inline kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName)
2019  {
2020  return m_pGraph->TryCloseLoop(pScan, rSensorName);
2021  }
2022 
2023  inline void CorrectPoses()
2024  {
2025  m_pGraph->CorrectPoses();
2026  }
2027 
2028  protected:
2029  void InitializeParameters();
2030 
2038  kt_bool HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const;
2039 
2040  public:
2042  // fire information for listeners!!
2043 
2048  void FireInfo(const std::string& rInfo) const;
2049 
2054  void FireDebug(const std::string& rInfo) const;
2055 
2060  void FireLoopClosureCheck(const std::string& rInfo) const;
2061 
2066  void FireBeginLoopClosure(const std::string& rInfo) const;
2067 
2072  void FireEndLoopClosure(const std::string& rInfo) const;
2073 
2074  // FireRunningScansUpdated
2075 
2076  // FireCovarianceCalculated
2077 
2078  // FireLoopClosureChain
2079 
2080  private:
2084  Mapper(const Mapper&);
2085 
2089  const Mapper& operator=(const Mapper&);
2090 
2091  public:
2092  void SetUseScanMatching(kt_bool val) { m_pUseScanMatching->SetValue(val); }
2093 
2094  protected:
2096 
2098 
2100 
2104 
2105 
2106  std::vector<MapperListener*> m_Listeners;
2107 
2117 
2122 
2135 
2145 
2155 
2165 
2172 
2178 
2185 
2191 
2198 
2206 
2213 
2219 
2225 
2227  // CorrelationParameters correlationParameters;
2228 
2234 
2240 
2246 
2247 
2249  // CorrelationParameters loopCorrelationParameters;
2250 
2256 
2262 
2268 
2270  // ScanMatcherParameters;
2271 
2272  // Variance of penalty for deviating from odometry when scan-matching.
2273  // The penalty is a multiplier (less than 1.0) is a function of the
2274  // delta of the scan position being tested and the odometric pose
2277 
2278  // The range of angles to search during a coarse search and a finer search
2281 
2282  // Resolution of angles to search during a coarse search
2284 
2285  // Minimum value of the penalty multiplier so scores do not
2286  // become too small
2289 
2290  // whether to increase the search space if no good matches are initially found
2292 
2293  friend class boost::serialization::access;
2294  template<class Archive>
2295  void serialize(Archive &ar, const unsigned int version)
2296  {
2297  std::cout << "Mapper <- Module\n";
2298  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Module);
2299  ar & BOOST_SERIALIZATION_NVP(m_Initialized);
2300  std::cout << "Mapper <- m_pSequentialScanMatcher\n";
2301  ar & BOOST_SERIALIZATION_NVP(m_pSequentialScanMatcher);
2302  std::cout << "Mapper <- m_pGraph\n";
2303  ar & BOOST_SERIALIZATION_NVP(m_pGraph);
2304  std::cout << "Mapper <- m_pMapperSensorManager\n";
2305  ar & BOOST_SERIALIZATION_NVP(m_pMapperSensorManager);
2306  std::cout << "Mapper <- m_Listeners\n";
2307  ar & BOOST_SERIALIZATION_NVP(m_Listeners);
2308  ar & BOOST_SERIALIZATION_NVP(m_pUseScanMatching);
2309  ar & BOOST_SERIALIZATION_NVP(m_pUseScanBarycenter);
2310  ar & BOOST_SERIALIZATION_NVP(m_pMinimumTimeInterval);
2311  ar & BOOST_SERIALIZATION_NVP(m_pMinimumTravelDistance);
2312  ar & BOOST_SERIALIZATION_NVP(m_pMinimumTravelHeading);
2313  ar & BOOST_SERIALIZATION_NVP(m_pScanBufferSize);
2314  ar & BOOST_SERIALIZATION_NVP(m_pScanBufferMaximumScanDistance);
2315  ar & BOOST_SERIALIZATION_NVP(m_pLinkMatchMinimumResponseFine);
2316  ar & BOOST_SERIALIZATION_NVP(m_pLinkScanMaximumDistance);
2317  ar & BOOST_SERIALIZATION_NVP(m_pDoLoopClosing);
2318  ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchMaximumDistance);
2319  ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumChainSize);
2320  ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMaximumVarianceCoarse);
2321  ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumResponseCoarse);
2322  ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumResponseFine);
2323  ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceDimension);
2324  ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceResolution);
2325  ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceSmearDeviation);
2326  ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceDimension);
2327  ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceResolution);
2328  ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceSmearDeviation);
2329  ar & BOOST_SERIALIZATION_NVP(m_pDistanceVariancePenalty);
2330  ar & BOOST_SERIALIZATION_NVP(m_pAngleVariancePenalty);
2331  ar & BOOST_SERIALIZATION_NVP(m_pFineSearchAngleOffset);
2332  ar & BOOST_SERIALIZATION_NVP(m_pCoarseSearchAngleOffset);
2333  ar & BOOST_SERIALIZATION_NVP(m_pCoarseAngleResolution);
2334  ar & BOOST_SERIALIZATION_NVP(m_pMinimumAnglePenalty);
2335  ar & BOOST_SERIALIZATION_NVP(m_pMinimumDistancePenalty);
2336  ar & BOOST_SERIALIZATION_NVP(m_pUseResponseExpansion);
2337  std::cout << "**Finished serializing Mapper**\n";
2338  }
2339  public:
2340  /* Abstract methods for parameter setters and getters */
2341 
2342  /* Getters */
2343  // General Parameters
2344  bool getParamUseScanMatching();
2345  bool getParamUseScanBarycenter();
2346  double getParamMinimumTimeInterval();
2347  double getParamMinimumTravelDistance();
2348  double getParamMinimumTravelHeading();
2349  int getParamScanBufferSize();
2350  double getParamScanBufferMaximumScanDistance();
2351  double getParamLinkMatchMinimumResponseFine();
2352  double getParamLinkScanMaximumDistance();
2353  double getParamLoopSearchMaximumDistance();
2354  bool getParamDoLoopClosing();
2355  int getParamLoopMatchMinimumChainSize();
2356  double getParamLoopMatchMaximumVarianceCoarse();
2357  double getParamLoopMatchMinimumResponseCoarse();
2358  double getParamLoopMatchMinimumResponseFine();
2359 
2360  // Correlation Parameters - Correlation Parameters
2361  double getParamCorrelationSearchSpaceDimension();
2362  double getParamCorrelationSearchSpaceResolution();
2363  double getParamCorrelationSearchSpaceSmearDeviation();
2364 
2365  // Correlation Parameters - Loop Closure Parameters
2366  double getParamLoopSearchSpaceDimension();
2367  double getParamLoopSearchSpaceResolution();
2368  double getParamLoopSearchSpaceSmearDeviation();
2369 
2370  // Scan Matcher Parameters
2371  double getParamDistanceVariancePenalty();
2372  double getParamAngleVariancePenalty();
2373  double getParamFineSearchAngleOffset();
2374  double getParamCoarseSearchAngleOffset();
2375  double getParamCoarseAngleResolution();
2376  double getParamMinimumAnglePenalty();
2377  double getParamMinimumDistancePenalty();
2378  bool getParamUseResponseExpansion();
2379 
2380  /* Setters */
2381  // General Parameters
2382  void setParamUseScanMatching(bool b);
2383  void setParamUseScanBarycenter(bool b);
2384  void setParamMinimumTimeInterval(double d);
2385  void setParamMinimumTravelDistance(double d);
2386  void setParamMinimumTravelHeading(double d);
2387  void setParamScanBufferSize(int i);
2388  void setParamScanBufferMaximumScanDistance(double d);
2389  void setParamLinkMatchMinimumResponseFine(double d);
2390  void setParamLinkScanMaximumDistance(double d);
2391  void setParamLoopSearchMaximumDistance(double d);
2392  void setParamDoLoopClosing(bool b);
2393  void setParamLoopMatchMinimumChainSize(int i);
2394  void setParamLoopMatchMaximumVarianceCoarse(double d);
2395  void setParamLoopMatchMinimumResponseCoarse(double d);
2396  void setParamLoopMatchMinimumResponseFine(double d);
2397 
2398  // Correlation Parameters - Correlation Parameters
2399  void setParamCorrelationSearchSpaceDimension(double d);
2400  void setParamCorrelationSearchSpaceResolution(double d);
2401  void setParamCorrelationSearchSpaceSmearDeviation(double d);
2402 
2403  // Correlation Parameters - Loop Closure Parameters
2404  void setParamLoopSearchSpaceDimension(double d);
2405  void setParamLoopSearchSpaceResolution(double d);
2406  void setParamLoopSearchSpaceSmearDeviation(double d);
2407 
2408  // Scan Matcher Parameters
2409  void setParamDistanceVariancePenalty(double d);
2410  void setParamAngleVariancePenalty(double d);
2411  void setParamFineSearchAngleOffset(double d);
2412  void setParamCoarseSearchAngleOffset(double d);
2413  void setParamCoarseAngleResolution(double d);
2414  void setParamMinimumAnglePenalty(double d);
2415  void setParamMinimumDistancePenalty(double d);
2416  void setParamUseResponseExpansion(bool b);
2417  };
2418  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Mapper)
2419 } // namespace karto
2420 
2421 #endif // karto_sdk_MAPPER_H
CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize, kt_double resolution, kt_double smearDeviation)
Definition: Mapper.h:1177
std::map< int, LocalizedRangeScan * > m_Scans
Definition: Mapper.h:1707
d
virtual void GetNodeOrientation(const int &unique_id, double &pose)
Definition: Mapper.h:1035
std::map< Name, ScanManager * > ScanManagerMap
Definition: Mapper.h:1519
virtual ~ScanSolver()
Definition: Mapper.h:959
#define NULL
EdgeLabel * m_pLabel
Definition: Mapper.h:474
ScanMatcher * m_pSequentialScanMatcher
Definition: Mapper.h:2097
T * m_pObject
Definition: Mapper.h:374
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:55
int32_t kt_int32s
Definition: Types.h:51
GraphTraversal(Graph< T > *pGraph)
Definition: Mapper.h:527
Matrix3 m_Covariance
Definition: Mapper.h:228
virtual ~CorrelationGrid()
Definition: Mapper.h:1060
Vertex< T > * m_pTarget
Definition: Mapper.h:473
EdgeLabel * GetLabel()
Definition: Mapper.h:457
Parameter< kt_double > * m_pLinkScanMaximumDistance
Definition: Mapper.h:2184
virtual void Info(const std::string &)
Definition: Mapper.h:52
kt_double Round(kt_double value)
Definition: Math.h:87
kt_bool TryCloseLoop(LocalizedRangeScan *pScan, const Name &rSensorName)
Definition: Mapper.h:2018
Mapper * m_pMapper
Definition: Mapper.h:1467
virtual void AddNode(Vertex< LocalizedRangeScan > *)
Definition: Mapper.h:978
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:380
Parameter< kt_double > * m_pCoarseAngleResolution
Definition: Mapper.h:2283
kt_int32s GetStateId() const
Definition: Karto.h:5143
Parameter< kt_double > * m_pLoopMatchMaximumVarianceCoarse
Definition: Mapper.h:2212
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:99
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1281
Parameter< kt_double > * m_pMinimumTravelHeading
Definition: Mapper.h:2154
virtual void EndLoopClosure(const std::string &)
Definition: Mapper.h:96
#define forEachAs(listtype, list, iter)
Definition: Macros.h:69
void AddEdge(Edge< T > *pEdge)
Definition: Mapper.h:620
Parameter< kt_double > * m_pMinimumAnglePenalty
Definition: Mapper.h:2287
Matrix3 Transpose() const
Definition: Karto.h:2525
std::vector< MapperListener * > m_Listeners
Definition: Mapper.h:2106
virtual void Clear()
Definition: Mapper.h:1006
virtual void RemoveNode(kt_int32s)
Definition: Mapper.h:985
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:478
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:129
Rectangle2< kt_int32s > m_Roi
Definition: Mapper.h:1280
uint8_t kt_int8u
Definition: Types.h:46
virtual void RemoveConstraint(kt_int32s, kt_int32s)
Definition: Mapper.h:999
virtual ~GraphTraversal()
Definition: Mapper.h:532
std::map< Name, std::map< int, Vertex< T > * > > VertexMap
Definition: Mapper.h:567
std::vector< Name > GetSensorNames()
Definition: Mapper.h:1563
Vertex< LocalizedRangeScan > * vertex
Definition: Mapper.h:1877
virtual void LoopClosureCheck(const std::string &)
Definition: Mapper.h:86
Parameter< kt_double > * m_pLoopSearchSpaceSmearDeviation
Definition: Mapper.h:2267
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:2295
void Update(const Pose2 &rPose1, const Pose2 &rPose2, const Matrix3 &rCovariance)
Definition: Mapper.h:172
const std::vector< Edge< T > * > & GetEdges() const
Definition: Mapper.h:665
const Matrix3 & GetCovariance()
Definition: Mapper.h:219
virtual std::unordered_map< int, Eigen::Vector3d > * getGraph()
Definition: Mapper.h:1020
Parameter< kt_int32u > * m_pLoopMatchMinimumChainSize
Definition: Mapper.h:2205
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Parameter< kt_double > * m_pCorrelationSearchSpaceResolution
Definition: Mapper.h:2239
void Clear()
Definition: Mapper.h:639
virtual kt_int32s GridIndex(const Vector2< kt_int32s > &rGrid, kt_bool boundaryCheck=true) const
Definition: Mapper.h:1102
Parameter< kt_double > * m_pMinimumDistancePenalty
Definition: Mapper.h:2288
void AddVertex(const Name &rName, Vertex< T > *pVertex)
Definition: Mapper.h:591
virtual void AddConstraint(Edge< LocalizedRangeScan > *)
Definition: Mapper.h:992
virtual ~Edge()
Definition: Mapper.h:422
Parameter< kt_double > * m_pDistanceVariancePenalty
Definition: Mapper.h:2275
MapperSensorManager * GetMapperSensorManager() const
Definition: Mapper.h:2008
Parameter< kt_bool > * m_pUseScanMatching
Definition: Mapper.h:2116
Vertex< T > * GetTarget() const
Definition: Mapper.h:448
#define forEach(listtype, list)
Definition: Macros.h:66
virtual void Reset()
Definition: Mapper.h:1013
std::vector< kt_double > m_xPoses
Definition: Mapper.h:1472
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:505
kt_double m_SmearDeviation
Definition: Mapper.h:1271
T * GetObject() const
Definition: Mapper.h:319
virtual void ModifyNode(const int &unique_id, Eigen::Vector3d pose)
Definition: Mapper.h:1028
Graph< T > * m_pGraph
Definition: Mapper.h:542
const double GetScore() const
Definition: Mapper.h:301
void SetROI(const Rectangle2< kt_int32s > &roi)
Definition: Mapper.h:1123
MapperSensorManager * m_pMapperSensorManager
Definition: Mapper.h:2099
kt_int32u m_RunningBufferMaximumSize
Definition: Mapper.h:1702
GraphTraversal< LocalizedRangeScan > * m_pTraversal
Definition: Mapper.h:913
Parameter< kt_bool > * m_pDoLoopClosing
Definition: Mapper.h:2190
friend class boost::serialization::access
Definition: Mapper.h:52
Parameter< kt_double > * m_pFineSearchAngleOffset
Definition: Mapper.h:2279
Parameter< kt_double > * m_pScanBufferMaximumScanDistance
Definition: Mapper.h:2171
ScanSolver * m_pScanOptimizer
Definition: Mapper.h:2102
kt_double m_Score
Definition: Mapper.h:376
LocalizedRangeScan * scan
Definition: Mapper.h:1875
const Pose2 & GetPose2()
Definition: Mapper.h:201
virtual void Debug(const std::string &)
Definition: Mapper.h:69
Parameter< kt_double > * m_pLoopSearchSpaceDimension
Definition: Mapper.h:2255
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:920
void RemoveEdge(const int &idx)
Definition: Mapper.h:290
virtual kt_int32s GridIndex(const Vector2< kt_int32s > &rGrid, kt_bool boundaryCheck=true) const
Definition: Karto.h:4755
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:72
const Name & GetSensorName() const
Definition: Karto.h:5197
void SmearPoint(const Vector2< kt_int32s > &rGridPoint)
Definition: Mapper.h:1132
CorrelationGrid * GetCorrelationGrid() const
Definition: Mapper.h:1415
void CorrectPoses()
Definition: Mapper.h:2023
Pose2 m_Pose2
Definition: Mapper.h:226
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:1687
Parameter< kt_double > * m_pMinimumTravelDistance
Definition: Mapper.h:2144
std::vector< Edge< T > * > m_Edges
Definition: Mapper.h:688
Mapper * m_pMapper
Definition: Mapper.h:903
kt_int8u * m_pKernel
Definition: Mapper.h:1277
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:1485
void SetScore(const double score)
Definition: Mapper.h:310
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:546
void AddEdge(Edge< T > *pEdge)
Definition: Mapper.h:369
void SetUseScanMatching(kt_bool val)
Definition: Mapper.h:2092
std::vector< Pose2 > Pose2Vector
Definition: Karto.h:2269
ScanMatcher * GetLoopScanMatcher() const
Definition: Mapper.h:806
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:232
virtual ~Vertex()
Definition: Mapper.h:274
void RemoveVertex(const Name &rName, const int &idx)
Definition: Mapper.h:601
virtual ~MapperSensorManager()
Definition: Mapper.h:1538
Vertex< T > * GetSource() const
Definition: Mapper.h:439
ScanMatcher(Mapper *pMapper)
Definition: Mapper.h:1457
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:172
kt_double m_RunningBufferMaximumDistance
Definition: Mapper.h:1703
Pose2 m_PoseDifference
Definition: Mapper.h:227
const T & GetX() const
Definition: Karto.h:1026
#define const_forEach(listtype, list)
Definition: Macros.h:72
VertexMap m_Vertices
Definition: Mapper.h:683
Vertex< T > * m_pSource
Definition: Mapper.h:472
LocalizationScanVertices m_LocalizationScanVertices
Definition: Mapper.h:2103
virtual void BeginLoopClosure(const std::string &)
Definition: Mapper.h:91
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:1286
Pose2 TransformPose(const Pose2 &rSourcePose)
Definition: Karto.h:3002
std::vector< Vertex< T > * > GetAdjacentVertices() const
Definition: Mapper.h:336
GridIndexLookup< kt_int8u > * m_pGridLookup
Definition: Mapper.h:1470
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:1042
bool kt_bool
Definition: Types.h:64
virtual ~Graph()
Definition: Mapper.h:580
static CorrelationGrid * CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution, kt_double smearDeviation)
Definition: Mapper.h:1081
CorrelationGrid * m_pCorrelationGrid
Definition: Mapper.h:1468
Parameter< kt_double > * m_pLoopSearchSpaceResolution
Definition: Mapper.h:2261
kt_bool m_Initialized
Definition: Mapper.h:2095
void RemoveEdge(const int &idx)
Definition: Mapper.h:629
Parameter< kt_double > * m_pCorrelationSearchSpaceDimension
Definition: Mapper.h:2233
MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
Definition: Mapper.h:1525
const std::vector< Edge< T > * > & GetEdges() const
Definition: Mapper.h:282
LocalizedRangeScan * GetScan(kt_int32s id)
Definition: Mapper.h:1592
kt_int32s m_KernelSize
Definition: Mapper.h:1274
std::vector< std::pair< kt_int32s, Pose2 > > IdPoseVector
Definition: Mapper.h:947
void RemoveObject()
Definition: Mapper.h:327
Parameter< kt_double > * m_pLinkMatchMinimumResponseFine
Definition: Mapper.h:2177
Vertex< LocalizedRangeScan > * GetVertex(LocalizedRangeScan *pScan)
Definition: Mapper.h:817
const Pose2 & GetPose1()
Definition: Mapper.h:192
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:694
virtual ~EdgeLabel()
Definition: Mapper.h:124
Pose2 m_Pose1
Definition: Mapper.h:225
std::pair< kt_double, Pose2 > * m_pPoseResponse
Definition: Mapper.h:1471
Pose2 m_rSearchCenter
Definition: Mapper.h:1474
ScanMatcher * m_pLoopScanMatcher
Definition: Mapper.h:908
Vertex(T *pObject)
Definition: Mapper.h:266
void SetLabel(EdgeLabel *pLabel)
Definition: Mapper.h:466
ScanManagerMap m_ScanManagers
Definition: Mapper.h:1700
Grid< kt_double > * m_pSearchSpaceProbs
Definition: Mapper.h:1469
std::vector< kt_double > m_yPoses
Definition: Mapper.h:1473
static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution)
Definition: Mapper.h:1259
Edge(Vertex< T > *pSource, Vertex< T > *pTarget)
Definition: Mapper.h:410
virtual ~LinkInfo()
Definition: Mapper.h:161
double kt_double
Definition: Types.h:67
Parameter< kt_double > * m_pCorrelationSearchSpaceSmearDeviation
Definition: Mapper.h:2245
Definition: Karto.h:86
const T & GetY() const
Definition: Karto.h:1044
MapperGraph * m_pGraph
Definition: Mapper.h:2101
Parameter< kt_double > * m_pLoopMatchMinimumResponseCoarse
Definition: Mapper.h:2218
const Rectangle2< kt_int32s > & GetROI() const
Definition: Mapper.h:1114
LinkInfo(const Pose2 &rPose1, const Pose2 &rPose2, const Matrix3 &rCovariance)
Definition: Mapper.h:153
const VertexMap & GetVertices() const
Definition: Mapper.h:674
Parameter< kt_double > * m_pCoarseSearchAngleOffset
Definition: Mapper.h:2280
Parameter< kt_double > * m_pLoopSearchMaximumDistance
Definition: Mapper.h:2197
kt_int32u m_nAngles
Definition: Mapper.h:1476
Parameter< kt_double > * m_pAngleVariancePenalty
Definition: Mapper.h:2276
uint32_t kt_int32u
Definition: Types.h:52
virtual void CalculateKernel()
Definition: Mapper.h:1195
Parameter< kt_double > * m_pMinimumTimeInterval
Definition: Mapper.h:2134
Parameter< kt_bool > * m_pUseResponseExpansion
Definition: Mapper.h:2291
kt_bool m_doPenalize
Definition: Mapper.h:1478
Parameter< kt_int32u > * m_pScanBufferSize
Definition: Mapper.h:2164
#define KARTO_EXPORT
Definition: Macros.h:56
kt_bool IsUpTo(const T &value, const T &maximum)
Definition: Math.h:147
kt_double m_searchAngleOffset
Definition: Mapper.h:1475
std::queue< LocalizationScanVertex > LocalizationScanVertices
Definition: Mapper.h:1880
std::vector< Edge< T > * > m_Edges
Definition: Mapper.h:375
ScanManager * GetScanManager(LocalizedRangeScan *pScan)
Definition: Mapper.h:1665
kt_double m_searchAngleResolution
Definition: Mapper.h:1477
std::vector< LocalizedRangeScan * > LocalizedRangeScanVector
Definition: Karto.h:5856
ScanManager * GetScanManager(const Name &rSensorName)
Definition: Mapper.h:1675
Parameter< kt_bool > * m_pUseScanBarycenter
Definition: Mapper.h:2121
std::map< int, LocalizedRangeScan * > LocalizedRangeScanMap
Definition: Karto.h:5857
const Pose2 & GetPoseDifference()
Definition: Mapper.h:210
Parameter< kt_double > * m_pLoopMatchMinimumResponseFine
Definition: Mapper.h:2224
kt_double GetHeading() const
Definition: Karto.h:2151
void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
Definition: Karto.h:2490


slam_toolbox
Author(s): Steve Macenski
autogenerated on Sat Oct 3 2020 03:51:01