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/parallel_do.h"
30 #include "tbb/blocked_range.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 
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 
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:
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 
815  void UpdateLoopScanMatcher(kt_double rangeThreshold);
816 
817  private:
824  {
825  Name rName = pScan->GetSensorName();
826  std::map<int, Vertex<LocalizedRangeScan>* >::iterator it = m_Vertices[rName].find(pScan->GetStateId());
827  if (it != m_Vertices[rName].end())
828  {
829  return it->second;
830  }
831  else
832  {
833  std::cout << "GetVertex: Failed to get vertex, idx " << pScan->GetStateId() <<
834  " is not in m_Vertices." << std::endl;
835  return nullptr;
836  }
837  }
838 
844  LocalizedRangeScan* GetClosestScanToPose(const LocalizedRangeScanVector& rScans, const Pose2& rPose) const;
845 
853  void LinkScans(LocalizedRangeScan* pFromScan,
854  LocalizedRangeScan* pToScan,
855  const Pose2& rMean,
856  const Matrix3& rCovariance);
857 
864  void LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector<Matrix3>& rCovariances);
865 
873  void LinkChainToScan(const LocalizedRangeScanVector& rChain,
874  LocalizedRangeScan* pScan,
875  const Pose2& rMean,
876  const Matrix3& rCovariance);
877 
883  std::vector<LocalizedRangeScanVector> FindNearChains(LocalizedRangeScan* pScan);
884 
891  Pose2 ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector<Matrix3>& rCovariances) const;
892 
901  LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan* pScan,
902  const Name& rSensorName,
903  kt_int32u& rStartNum);
904 
905  private:
910 
915 
920 
924  friend class boost::serialization::access;
925  template<class Archive>
926  void serialize(Archive &ar, const unsigned int version)
927  {
928  std::cout << "MapperGraph <- Graph; ";
929  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Graph<LocalizedRangeScan>);
930  std::cout << "MapperGraph <- m_pMapper; ";
931  ar & BOOST_SERIALIZATION_NVP(m_pMapper);
932  std::cout << "MapperGraph <- m_pLoopScanMatcher; ";
933  ar & BOOST_SERIALIZATION_NVP(m_pLoopScanMatcher);
934  std::cout << "MapperGraph <- m_pTraversal\n";
935  ar & BOOST_SERIALIZATION_NVP(m_pTraversal);
936  }
937 
938  }; // MapperGraph
939 
943 
948  {
949  public:
953  typedef std::vector<std::pair<kt_int32s, Pose2> > IdPoseVector;
954 
959  {
960  }
961 
965  virtual ~ScanSolver()
966  {
967  }
968 
969  public:
973  virtual void Compute() = 0;
974 
979  virtual const IdPoseVector& GetCorrections() const = 0;
980 
984  virtual void AddNode(Vertex<LocalizedRangeScan>* /*pVertex*/)
985  {
986  }
987 
991  virtual void RemoveNode(kt_int32s /*id*/)
992  {
993  }
994 
998  virtual void AddConstraint(Edge<LocalizedRangeScan>* /*pEdge*/)
999  {
1000  }
1001 
1005  virtual void RemoveConstraint(kt_int32s /*sourceId*/, kt_int32s /*targetId*/)
1006  {
1007  }
1008 
1012  virtual void Clear()
1013  {
1014  }
1015 
1019  virtual void Reset()
1020  {
1021  }
1022 
1026  virtual std::unordered_map<int, Eigen::Vector3d>* getGraph()
1027  {
1028  std::cout << "getGraph method not implemented for this solver type. Graph visualization unavailable." << std::endl;
1029  return nullptr;
1030  }
1031 
1035  virtual void ModifyNode(const int& unique_id, Eigen::Vector3d pose)
1036  {
1037  std::cout << "ModifyNode method not implemented for this solver type. Manual loop closure unavailable." << std::endl;
1038  };
1042  virtual void GetNodeOrientation(const int& unique_id, double& pose)
1043  {
1044  std::cout << "GetNodeOrientation method not implemented for this solver type." << std::endl;
1045  };
1046 
1047  friend class boost::serialization::access;
1048  template<class Archive>
1049  void serialize(Archive &ar, const unsigned int version)
1050  {
1051  }
1052  }; // ScanSolver
1053  BOOST_SERIALIZATION_ASSUME_ABSTRACT(ScanSolver)
1057 
1062  {
1063  public:
1068  {
1069  if (m_pKernel)
1070  {
1071  delete [] m_pKernel;
1072  }
1073 
1074  }
1075 
1076  public:
1086  {
1087  }
1089  kt_int32s height,
1090  kt_double resolution,
1091  kt_double smearDeviation)
1092  {
1093  assert(resolution != 0.0);
1094 
1095  // +1 in case of roundoff
1096  kt_int32u borderSize = GetHalfKernelSize(smearDeviation, resolution) + 1;
1097 
1098  CorrelationGrid* pGrid = new CorrelationGrid(width, height, borderSize, resolution, smearDeviation);
1099 
1100  return pGrid;
1101  }
1102 
1109  virtual kt_int32s GridIndex(const Vector2<kt_int32s>& rGrid, kt_bool boundaryCheck = true) const
1110  {
1111  kt_int32s x = rGrid.GetX() + m_Roi.GetX();
1112  kt_int32s y = rGrid.GetY() + m_Roi.GetY();
1113 
1114  return Grid<kt_int8u>::GridIndex(Vector2<kt_int32s>(x, y), boundaryCheck);
1115  }
1116 
1121  inline const Rectangle2<kt_int32s>& GetROI() const
1122  {
1123  return m_Roi;
1124  }
1125 
1130  inline void SetROI(const Rectangle2<kt_int32s>& roi)
1131  {
1132  m_Roi = roi;
1133  }
1134 
1139  inline void SmearPoint(const Vector2<kt_int32s>& rGridPoint)
1140  {
1141  assert(m_pKernel != NULL);
1142 
1143  int gridIndex = GridIndex(rGridPoint);
1144  if (GetDataPointer()[gridIndex] != GridStates_Occupied)
1145  {
1146  return;
1147  }
1148 
1149  kt_int32s halfKernel = m_KernelSize / 2;
1150 
1151  // apply kernel
1152  for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
1153  {
1154  kt_int8u* pGridAdr = GetDataPointer(Vector2<kt_int32s>(rGridPoint.GetX(), rGridPoint.GetY() + j));
1155 
1156  kt_int32s kernelConstant = (halfKernel) + m_KernelSize * (j + halfKernel);
1157 
1158  // if a point is on the edge of the grid, there is no problem
1159  // with running over the edge of allowable memory, because
1160  // the grid has margins to compensate for the kernel size
1161  for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
1162  {
1163  kt_int32s kernelArrayIndex = i + kernelConstant;
1164 
1165  kt_int8u kernelValue = m_pKernel[kernelArrayIndex];
1166  if (kernelValue > pGridAdr[i])
1167  {
1168  // kernel value is greater, so set it to kernel value
1169  pGridAdr[i] = kernelValue;
1170  }
1171  }
1172  }
1173  }
1174 
1175  protected:
1184  CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize,
1185  kt_double resolution, kt_double smearDeviation)
1186  : Grid<kt_int8u>(width + borderSize * 2, height + borderSize * 2)
1187  , m_SmearDeviation(smearDeviation)
1188  , m_pKernel(NULL)
1189  {
1190  GetCoordinateConverter()->SetScale(1.0 / resolution);
1191 
1192  // setup region of interest
1193  m_Roi = Rectangle2<kt_int32s>(borderSize, borderSize, width, height);
1194 
1195  // calculate kernel
1196  CalculateKernel();
1197  }
1198 
1202  virtual void CalculateKernel()
1203  {
1204  kt_double resolution = GetResolution();
1205 
1206  assert(resolution != 0.0);
1207  assert(m_SmearDeviation != 0.0);
1208 
1209  // min and max distance deviation for smearing;
1210  // will smear for two standard deviations, so deviation must be at least 1/2 of the resolution
1211  const kt_double MIN_SMEAR_DISTANCE_DEVIATION = 0.5 * resolution;
1212  const kt_double MAX_SMEAR_DISTANCE_DEVIATION = 10 * resolution;
1213 
1214  // check if given value too small or too big
1215  if (!math::InRange(m_SmearDeviation, MIN_SMEAR_DISTANCE_DEVIATION, MAX_SMEAR_DISTANCE_DEVIATION))
1216  {
1217  std::stringstream error;
1218  error << "Mapper Error: Smear deviation too small: Must be between "
1219  << MIN_SMEAR_DISTANCE_DEVIATION
1220  << " and "
1221  << MAX_SMEAR_DISTANCE_DEVIATION;
1222  throw std::runtime_error(error.str());
1223  }
1224 
1225  // NOTE: Currently assumes a two-dimensional kernel
1226 
1227  // +1 for center
1228  m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1;
1229 
1230  // allocate kernel
1231  m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];
1232  if (m_pKernel == NULL)
1233  {
1234  throw std::runtime_error("Unable to allocate memory for kernel!");
1235  }
1236 
1237  // calculate kernel
1238  kt_int32s halfKernel = m_KernelSize / 2;
1239  for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
1240  {
1241  for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
1242  {
1243 #ifdef WIN32
1244  kt_double distanceFromMean = _hypot(i * resolution, j * resolution);
1245 #else
1246  kt_double distanceFromMean = hypot(i * resolution, j * resolution);
1247 #endif
1248  kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2));
1249 
1250  kt_int32u kernelValue = static_cast<kt_int32u>(math::Round(z * GridStates_Occupied));
1251  assert(math::IsUpTo(kernelValue, static_cast<kt_int32u>(255)));
1252 
1253  int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel);
1254  m_pKernel[kernelArrayIndex] = static_cast<kt_int8u>(kernelValue);
1255  }
1256  }
1257  }
1258 
1266  static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution)
1267  {
1268  assert(resolution != 0.0);
1269 
1270  return static_cast<kt_int32s>(math::Round(2.0 * smearDeviation / resolution));
1271  }
1272 
1273  private:
1279 
1280  // Size of one side of the kernel
1282 
1283  // Cached kernel for smearing
1285 
1286  // region of interest
1291  friend class boost::serialization::access;
1292  template<class Archive>
1293  void serialize(Archive &ar, const unsigned int version)
1294  {
1295  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Grid<kt_int8u>);
1296  ar & BOOST_SERIALIZATION_NVP(m_SmearDeviation);
1297  ar & BOOST_SERIALIZATION_NVP(m_KernelSize);
1298  if (Archive::is_loading::value)
1299  {
1300  m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];
1301  }
1302  ar & boost::serialization::make_array<kt_int8u>(m_pKernel, m_KernelSize * m_KernelSize);
1303  ar & BOOST_SERIALIZATION_NVP(m_Roi);
1304  }
1305  }; // CorrelationGrid
1306  BOOST_SERIALIZATION_ASSUME_ABSTRACT(CorrelationGrid)
1310 
1315  {
1316  public:
1318  {
1319  }
1323  virtual ~ScanMatcher();
1324 
1325  public:
1329  void operator() (const kt_double& y) const;
1330 
1334  static ScanMatcher* Create(Mapper* pMapper,
1335  kt_double searchSize,
1336  kt_double resolution,
1337  kt_double smearDeviation,
1338  kt_double rangeThreshold);
1339 
1350  template<class T = LocalizedRangeScanVector>
1351  kt_double MatchScan(LocalizedRangeScan* pScan,
1352  const T& rBaseScans,
1353  Pose2& rMean, Matrix3& rCovariance,
1354  kt_bool doPenalize = true,
1355  kt_bool doRefineMatch = true);
1356 
1373  kt_double CorrelateScan(LocalizedRangeScan* pScan,
1374  const Pose2& rSearchCenter,
1375  const Vector2<kt_double>& rSearchSpaceOffset,
1376  const Vector2<kt_double>& rSearchSpaceResolution,
1377  kt_double searchAngleOffset,
1378  kt_double searchAngleResolution,
1379  kt_bool doPenalize,
1380  Pose2& rMean,
1381  Matrix3& rCovariance,
1382  kt_bool doingFineMatch);
1383 
1394  void ComputePositionalCovariance(const Pose2& rBestPose,
1395  kt_double bestResponse,
1396  const Pose2& rSearchCenter,
1397  const Vector2<kt_double>& rSearchSpaceOffset,
1398  const Vector2<kt_double>& rSearchSpaceResolution,
1399  kt_double searchAngleResolution,
1400  Matrix3& rCovariance);
1401 
1411  void ComputeAngularCovariance(const Pose2& rBestPose,
1412  kt_double bestResponse,
1413  const Pose2& rSearchCenter,
1414  kt_double searchAngleOffset,
1415  kt_double searchAngleResolution,
1416  Matrix3& rCovariance);
1417 
1423  {
1424  return m_pCorrelationGrid;
1425  }
1426 
1427  private:
1433  void AddScans(const LocalizedRangeScanVector& rScans, Vector2<kt_double> viewPoint);
1434  void AddScans(const LocalizedRangeScanMap& rScans, Vector2<kt_double> viewPoint);
1435 
1442  void AddScan(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint, kt_bool doSmear = true);
1443 
1450  PointVectorDouble FindValidPoints(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint) const;
1451 
1458  kt_double GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const;
1459 
1460  protected:
1465  : m_pMapper(pMapper)
1466  , m_pCorrelationGrid(NULL)
1467  , m_pSearchSpaceProbs(NULL)
1468  , m_pGridLookup(NULL)
1469  , m_pPoseResponse(NULL)
1470  , m_doPenalize(false)
1471  {
1472  }
1473 
1474  private:
1479  std::pair<kt_double, Pose2>* m_pPoseResponse;
1480  std::vector<kt_double> m_xPoses;
1481  std::vector<kt_double> m_yPoses;
1487 
1491  friend class boost::serialization::access;
1492  template<class Archive>
1493  void serialize(Archive &ar, const unsigned int version)
1494  {
1495  ar & BOOST_SERIALIZATION_NVP(m_pMapper);
1496  ar & BOOST_SERIALIZATION_NVP(m_pCorrelationGrid);
1497  ar & BOOST_SERIALIZATION_NVP(m_pSearchSpaceProbs);
1498  ar & BOOST_SERIALIZATION_NVP(m_pGridLookup);
1499  ar & BOOST_SERIALIZATION_NVP(m_xPoses);
1500  ar & BOOST_SERIALIZATION_NVP(m_yPoses);
1501  ar & BOOST_SERIALIZATION_NVP(m_rSearchCenter);
1502  ar & BOOST_SERIALIZATION_NVP(m_searchAngleResolution);
1503  ar & BOOST_SERIALIZATION_NVP(m_nAngles);
1504  ar & BOOST_SERIALIZATION_NVP(m_searchAngleResolution);;
1505  ar & BOOST_SERIALIZATION_NVP(m_doPenalize);
1506 
1507  // Note - m_pPoseResponse is generally only ever defined within the
1508  // execution of ScanMatcher::CorrelateScan and used as a temporary
1509  // accumulator for multithreaded matching results. It would normally
1510  // not make sense to serialize, but we don't want to break compatibility
1511  // with previously serialized data. Gen some dummy data that we free
1512  // immediately after so that we don't alloc here and leak.
1513  kt_int32u poseResponseSize =
1514  static_cast<kt_int32u>(m_xPoses.size() * m_yPoses.size() * m_nAngles);
1515 
1516  // We could check first if m_pPoseResponse == nullptr for good measure, but
1517  // based on the codepaths it should always be freed and set to null outside of
1518  // any execution of ScanMatcher::CorrelateScan, so go ahead and alloc here.
1519  m_pPoseResponse = new std::pair<kt_double, Pose2>[poseResponseSize];
1520  ar & boost::serialization::make_array<std::pair<kt_double, Pose2>>(m_pPoseResponse,
1521  poseResponseSize);
1522 
1523  // Aaaand now, clean up the dummy data
1524  delete[] m_pPoseResponse;
1525  m_pPoseResponse = nullptr;
1526  }
1527 
1528  }; // ScanMatcher
1529 
1533 
1534  class ScanManager;
1535 
1539  class KARTO_EXPORT MapperSensorManager //: public SensorManager // was commented out, works with it in, but I'll leave out for now...
1540  {
1541  typedef std::map<Name, ScanManager*> ScanManagerMap;
1542 
1543  public:
1547  MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
1548  : m_RunningBufferMaximumSize(runningBufferMaximumSize)
1549  , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
1550  , m_NextScanId(0)
1551  {
1552  }
1553 
1555  }
1556 
1561  {
1562  Clear();
1563  }
1564 
1565  public:
1571  void RegisterSensor(const Name& rSensorName);
1572 
1579  LocalizedRangeScan* GetScan(const Name& rSensorName, kt_int32s scanIndex);
1580 
1585  inline std::vector<Name> GetSensorNames()
1586  {
1587  std::vector<Name> deviceNames;
1588  const_forEach(ScanManagerMap, &m_ScanManagers)
1589  {
1590  deviceNames.push_back(iter->first);
1591  }
1592 
1593  return deviceNames;
1594  }
1595 
1601  LocalizedRangeScan* GetLastScan(const Name& rSensorName);
1602 
1607  void SetLastScan(LocalizedRangeScan* pScan);
1608 
1613  void ClearLastScan(LocalizedRangeScan* pScan);
1614 
1619  void ClearLastScan(const Name& name);
1620 
1627  {
1628  std::map<int, LocalizedRangeScan*>::iterator it = m_Scans.find(id);
1629  if (it != m_Scans.end())
1630  {
1631  return it->second;
1632  }
1633  else
1634  {
1635  std::cout << "GetScan: id " << id <<
1636  " does not exist in m_scans, cannot retrieve it." << std::endl;
1637  return nullptr;
1638  }
1639  }
1640 
1645  void AddScan(LocalizedRangeScan* pScan);
1646 
1651  void AddRunningScan(LocalizedRangeScan* pScan);
1652 
1657  void RemoveScan(LocalizedRangeScan* pScan);
1658 
1664  LocalizedRangeScanMap& GetScans(const Name& rSensorName);
1665 
1671  LocalizedRangeScanVector& GetRunningScans(const Name& rSensorName);
1672 
1676  void ClearRunningScans(const Name& rSensorName);
1677 
1681  kt_int32u GetRunningScanBufferSize(const Name& rSensorName);
1682 
1687  void SetRunningScanBufferSize(kt_int32u rScanBufferSize);
1688 
1693  void SetRunningScanBufferMaximumDistance(kt_double rScanBufferMaxDistance);
1694 
1699  LocalizedRangeScanVector GetAllScans();
1700 
1704  void Clear();
1705 
1706  private:
1712  {
1713  return GetScanManager(pScan->GetSensorName());
1714  }
1715 
1721  inline ScanManager* GetScanManager(const Name& rSensorName)
1722  {
1723  if (m_ScanManagers.find(rSensorName) != m_ScanManagers.end())
1724  {
1725  return m_ScanManagers[rSensorName];
1726  }
1727 
1728  return NULL;
1729  }
1730 
1731  friend class boost::serialization::access;
1732  template<class Archive>
1733  void serialize(Archive &ar, const unsigned int version)
1734  {
1735  std::cout << "MapperSensorManager <- m_ScanManagers; ";
1736  ar & BOOST_SERIALIZATION_NVP(m_ScanManagers);
1737  ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumSize);
1738  ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumDistance);
1739  ar & BOOST_SERIALIZATION_NVP(m_NextScanId);
1740  std::cout << "MapperSensorManager <- m_Scans\n";
1741  ar & BOOST_SERIALIZATION_NVP(m_Scans);
1742  }
1743 
1744  private:
1745  // map from device ID to scan data
1747 
1750 
1752 
1753  std::map<int, LocalizedRangeScan*> m_Scans;
1754  }; // MapperSensorManager
1755 
1759 
1919  {
1924  };
1925 
1926  typedef std::queue<LocalizationScanVertex> LocalizationScanVertices;
1927 
1928  class KARTO_EXPORT Mapper : public Module
1929  {
1930  friend class MapperGraph;
1931  friend class ScanMatcher;
1932 
1933  public:
1937  Mapper();
1938 
1943  Mapper(const std::string& rName);
1944 
1948  virtual ~Mapper();
1949 
1950  public:
1955  void Initialize(kt_double rangeThreshold);
1956 
1961  void SaveToFile(const std::string& filename);
1962 
1967  void LoadFromFile(const std::string& filename);
1968 
1973  virtual void Reset();
1974 
1986  virtual kt_bool Process(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
1987 
1991  virtual kt_bool Process(Object* pObject);
1992 
1993  // processors
1994  kt_bool ProcessAtDock(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
1995  kt_bool ProcessAgainstNode(LocalizedRangeScan * pScan, const int & nodeId, Matrix3 * covariance = nullptr);
1996  kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan* pScan, kt_bool addScanToLocalizationBuffer = false, Matrix3 * covariance = nullptr);
1997  kt_bool ProcessLocalization(LocalizedRangeScan * pScan, Matrix3 * covariance = nullptr);
1998  kt_bool RemoveNodeFromGraph(Vertex<LocalizedRangeScan>*);
1999  void AddScanToLocalizationBuffer(LocalizedRangeScan* pScan, Vertex<LocalizedRangeScan>* scan_vertex);
2000  void ClearLocalizationBuffer();
2001 
2008  virtual const LocalizedRangeScanVector GetAllProcessedScans() const;
2009 
2014  void AddListener(MapperListener* pListener);
2015 
2020  void RemoveListener(MapperListener* pListener);
2021 
2026  void SetScanSolver(ScanSolver* pSolver);
2027 
2032  ScanSolver* getScanSolver();
2033 
2038  virtual MapperGraph* GetGraph() const;
2039 
2044  virtual ScanMatcher* GetSequentialScanMatcher() const;
2045 
2050  virtual ScanMatcher* GetLoopScanMatcher() const;
2051 
2057  {
2058  return m_pMapperSensorManager;
2059  }
2060 
2066  inline kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName)
2067  {
2068  return m_pGraph->TryCloseLoop(pScan, rSensorName);
2069  }
2070 
2071  inline void CorrectPoses()
2072  {
2073  m_pGraph->CorrectPoses();
2074  }
2075 
2076  protected:
2077  void InitializeParameters();
2078 
2086  kt_bool HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const;
2087 
2088  public:
2090  // fire information for listeners!!
2091 
2096  void FireInfo(const std::string& rInfo) const;
2097 
2102  void FireDebug(const std::string& rInfo) const;
2103 
2108  void FireLoopClosureCheck(const std::string& rInfo) const;
2109 
2114  void FireBeginLoopClosure(const std::string& rInfo) const;
2115 
2120  void FireEndLoopClosure(const std::string& rInfo) const;
2121 
2122  // FireRunningScansUpdated
2123 
2124  // FireCovarianceCalculated
2125 
2126  // FireLoopClosureChain
2127 
2128  private:
2132  Mapper(const Mapper&);
2133 
2137  const Mapper& operator=(const Mapper&);
2138 
2139  public:
2140  void SetUseScanMatching(kt_bool val) { m_pUseScanMatching->SetValue(val); }
2141 
2142  protected:
2145 
2147 
2149 
2153 
2154 
2155  std::vector<MapperListener*> m_Listeners;
2156 
2166 
2171 
2184 
2194 
2204 
2214 
2221 
2227 
2234 
2240 
2247 
2255 
2262 
2268 
2274 
2276  // CorrelationParameters correlationParameters;
2277 
2283 
2289 
2295 
2296 
2298  // CorrelationParameters loopCorrelationParameters;
2299 
2305 
2311 
2317 
2319  // ScanMatcherParameters;
2320 
2321  // Variance of penalty for deviating from odometry when scan-matching.
2322  // The penalty is a multiplier (less than 1.0) is a function of the
2323  // delta of the scan position being tested and the odometric pose
2326 
2327  // The range of angles to search during a coarse search and a finer search
2330 
2331  // Resolution of angles to search during a coarse search
2333 
2334  // Minimum value of the penalty multiplier so scores do not
2335  // become too small
2338 
2339  // whether to increase the search space if no good matches are initially found
2341 
2342  friend class boost::serialization::access;
2343  template<class Archive>
2344  void serialize(Archive &ar, const unsigned int version)
2345  {
2346  std::cout << "Mapper <- Module\n";
2347  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Module);
2348  ar & BOOST_SERIALIZATION_NVP(m_Initialized);
2349  std::cout << "Mapper <- m_pSequentialScanMatcher\n";
2350  ar & BOOST_SERIALIZATION_NVP(m_pSequentialScanMatcher);
2351  std::cout << "Mapper <- m_pGraph\n";
2352  ar & BOOST_SERIALIZATION_NVP(m_pGraph);
2353  std::cout << "Mapper <- m_pMapperSensorManager\n";
2354  ar & BOOST_SERIALIZATION_NVP(m_pMapperSensorManager);
2355  std::cout << "Mapper <- m_Listeners\n";
2356  ar & BOOST_SERIALIZATION_NVP(m_Listeners);
2357  ar & BOOST_SERIALIZATION_NVP(m_pUseScanMatching);
2358  ar & BOOST_SERIALIZATION_NVP(m_pUseScanBarycenter);
2359  ar & BOOST_SERIALIZATION_NVP(m_pMinimumTimeInterval);
2360  ar & BOOST_SERIALIZATION_NVP(m_pMinimumTravelDistance);
2361  ar & BOOST_SERIALIZATION_NVP(m_pMinimumTravelHeading);
2362  ar & BOOST_SERIALIZATION_NVP(m_pScanBufferSize);
2363  ar & BOOST_SERIALIZATION_NVP(m_pScanBufferMaximumScanDistance);
2364  ar & BOOST_SERIALIZATION_NVP(m_pLinkMatchMinimumResponseFine);
2365  ar & BOOST_SERIALIZATION_NVP(m_pLinkScanMaximumDistance);
2366  ar & BOOST_SERIALIZATION_NVP(m_pDoLoopClosing);
2367  ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchMaximumDistance);
2368  ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumChainSize);
2369  ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMaximumVarianceCoarse);
2370  ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumResponseCoarse);
2371  ar & BOOST_SERIALIZATION_NVP(m_pLoopMatchMinimumResponseFine);
2372  ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceDimension);
2373  ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceResolution);
2374  ar & BOOST_SERIALIZATION_NVP(m_pCorrelationSearchSpaceSmearDeviation);
2375  ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceDimension);
2376  ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceResolution);
2377  ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceSmearDeviation);
2378  ar & BOOST_SERIALIZATION_NVP(m_pDistanceVariancePenalty);
2379  ar & BOOST_SERIALIZATION_NVP(m_pAngleVariancePenalty);
2380  ar & BOOST_SERIALIZATION_NVP(m_pFineSearchAngleOffset);
2381  ar & BOOST_SERIALIZATION_NVP(m_pCoarseSearchAngleOffset);
2382  ar & BOOST_SERIALIZATION_NVP(m_pCoarseAngleResolution);
2383  ar & BOOST_SERIALIZATION_NVP(m_pMinimumAnglePenalty);
2384  ar & BOOST_SERIALIZATION_NVP(m_pMinimumDistancePenalty);
2385  ar & BOOST_SERIALIZATION_NVP(m_pUseResponseExpansion);
2386  std::cout << "**Finished serializing Mapper**\n";
2387  }
2388  public:
2389  /* Abstract methods for parameter setters and getters */
2390 
2391  /* Getters */
2392  // General Parameters
2393  bool getParamUseScanMatching();
2394  bool getParamUseScanBarycenter();
2395  double getParamMinimumTimeInterval();
2396  double getParamMinimumTravelDistance();
2397  double getParamMinimumTravelHeading();
2398  int getParamScanBufferSize();
2399  double getParamScanBufferMaximumScanDistance();
2400  double getParamLinkMatchMinimumResponseFine();
2401  double getParamLinkScanMaximumDistance();
2402  double getParamLoopSearchMaximumDistance();
2403  bool getParamDoLoopClosing();
2404  int getParamLoopMatchMinimumChainSize();
2405  double getParamLoopMatchMaximumVarianceCoarse();
2406  double getParamLoopMatchMinimumResponseCoarse();
2407  double getParamLoopMatchMinimumResponseFine();
2408 
2409  // Correlation Parameters - Correlation Parameters
2410  double getParamCorrelationSearchSpaceDimension();
2411  double getParamCorrelationSearchSpaceResolution();
2412  double getParamCorrelationSearchSpaceSmearDeviation();
2413 
2414  // Correlation Parameters - Loop Closure Parameters
2415  double getParamLoopSearchSpaceDimension();
2416  double getParamLoopSearchSpaceResolution();
2417  double getParamLoopSearchSpaceSmearDeviation();
2418 
2419  // Scan Matcher Parameters
2420  double getParamDistanceVariancePenalty();
2421  double getParamAngleVariancePenalty();
2422  double getParamFineSearchAngleOffset();
2423  double getParamCoarseSearchAngleOffset();
2424  double getParamCoarseAngleResolution();
2425  double getParamMinimumAnglePenalty();
2426  double getParamMinimumDistancePenalty();
2427  bool getParamUseResponseExpansion();
2428 
2429  /* Setters */
2430  // General Parameters
2431  void setParamUseScanMatching(bool b);
2432  void setParamUseScanBarycenter(bool b);
2433  void setParamMinimumTimeInterval(double d);
2434  void setParamMinimumTravelDistance(double d);
2435  void setParamMinimumTravelHeading(double d);
2436  void setParamScanBufferSize(int i);
2437  void setParamScanBufferMaximumScanDistance(double d);
2438  void setParamLinkMatchMinimumResponseFine(double d);
2439  void setParamLinkScanMaximumDistance(double d);
2440  void setParamLoopSearchMaximumDistance(double d);
2441  void setParamDoLoopClosing(bool b);
2442  void setParamLoopMatchMinimumChainSize(int i);
2443  void setParamLoopMatchMaximumVarianceCoarse(double d);
2444  void setParamLoopMatchMinimumResponseCoarse(double d);
2445  void setParamLoopMatchMinimumResponseFine(double d);
2446 
2447  // Correlation Parameters - Correlation Parameters
2448  void setParamCorrelationSearchSpaceDimension(double d);
2449  void setParamCorrelationSearchSpaceResolution(double d);
2450  void setParamCorrelationSearchSpaceSmearDeviation(double d);
2451 
2452  // Correlation Parameters - Loop Closure Parameters
2453  void setParamLoopSearchSpaceDimension(double d);
2454  void setParamLoopSearchSpaceResolution(double d);
2455  void setParamLoopSearchSpaceSmearDeviation(double d);
2456 
2457  // Scan Matcher Parameters
2458  void setParamDistanceVariancePenalty(double d);
2459  void setParamAngleVariancePenalty(double d);
2460  void setParamFineSearchAngleOffset(double d);
2461  void setParamCoarseSearchAngleOffset(double d);
2462  void setParamCoarseAngleResolution(double d);
2463  void setParamMinimumAnglePenalty(double d);
2464  void setParamMinimumDistancePenalty(double d);
2465  void setParamUseResponseExpansion(bool b);
2466  };
2467  BOOST_SERIALIZATION_ASSUME_ABSTRACT(Mapper)
2468 } // namespace karto
2469 
2470 #endif // karto_sdk_MAPPER_H
karto::MapperDebugListener
Definition: Mapper.h:63
karto::Name
Definition: Karto.h:389
const_forEach
#define const_forEach(listtype, list)
Definition: Macros.h:72
karto::math::Round
kt_double Round(kt_double value)
Definition: Math.h:87
karto::ScanMatcher::m_pGridLookup
GridIndexLookup< kt_int8u > * m_pGridLookup
Definition: Mapper.h:1478
karto::Mapper
Definition: Mapper.h:1928
karto::Mapper::m_pAngleVariancePenalty
Parameter< kt_double > * m_pAngleVariancePenalty
Definition: Mapper.h:2325
karto::ScanSolver
Definition: Mapper.h:947
karto::ScanSolver::AddConstraint
virtual void AddConstraint(Edge< LocalizedRangeScan > *)
Definition: Mapper.h:998
karto::Transform::TransformPose
Pose2 TransformPose(const Pose2 &rSourcePose)
Definition: Karto.h:3004
karto::MapperGraph::MapperGraph
MapperGraph()
Definition: Mapper.h:722
karto::Rectangle2< kt_int32s >
karto::ScanSolver::RemoveConstraint
virtual void RemoveConstraint(kt_int32s, kt_int32s)
Definition: Mapper.h:1005
karto::Edge::access
friend class boost::serialization::access
Definition: Mapper.h:476
karto::Mapper::m_pDoLoopClosing
Parameter< kt_bool > * m_pDoLoopClosing
Definition: Mapper.h:2239
karto::Vertex::m_Edges
std::vector< Edge< T > * > m_Edges
Definition: Mapper.h:375
karto::ScanMatcher::m_rSearchCenter
Pose2 m_rSearchCenter
Definition: Mapper.h:1482
karto::Pose2Vector
std::vector< Pose2 > Pose2Vector
Definition: Karto.h:2271
karto::Visitor::Visit
virtual kt_bool Visit(Vertex< T > *pVertex)=0
karto::Edge::Edge
Edge(Vertex< T > *pSource, Vertex< T > *pTarget)
Definition: Mapper.h:410
karto::LinkInfo::GetPose1
const Pose2 & GetPose1()
Definition: Mapper.h:192
karto::Visitor
Definition: Mapper.h:494
karto::CorrelationGrid::SetROI
void SetROI(const Rectangle2< kt_int32s > &roi)
Definition: Mapper.h:1130
kt_double
double kt_double
Definition: Types.h:67
karto::Vertex::m_Score
kt_double m_Score
Definition: Mapper.h:376
karto::MapperGraph::GetLoopScanMatcher
ScanMatcher * GetLoopScanMatcher() const
Definition: Mapper.h:806
karto::Vertex::AddEdge
void AddEdge(Edge< T > *pEdge)
Definition: Mapper.h:369
karto::LocalizationScanVertex::scan
LocalizedRangeScan * scan
Definition: Mapper.h:1921
karto::Module
Definition: Karto.h:818
karto::Graph::m_Edges
std::vector< Edge< T > * > m_Edges
Definition: Mapper.h:688
karto::Vertex::Vertex
Vertex()
Definition: Mapper.h:262
karto::ScanSolver::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:1049
karto::Mapper::m_pLoopSearchMaximumDistance
Parameter< kt_double > * m_pLoopSearchMaximumDistance
Definition: Mapper.h:2246
karto::Mapper::CorrectPoses
void CorrectPoses()
Definition: Mapper.h:2071
karto::ScanSolver::ModifyNode
virtual void ModifyNode(const int &unique_id, Eigen::Vector3d pose)
Definition: Mapper.h:1035
karto::LinkInfo::Update
void Update(const Pose2 &rPose1, const Pose2 &rPose2, const Matrix3 &rCovariance)
Definition: Mapper.h:172
karto::MapperGraph::m_pMapper
Mapper * m_pMapper
Definition: Mapper.h:909
karto::ScanSolver::Compute
virtual void Compute()=0
karto::MapperSensorManager::MapperSensorManager
MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
Definition: Mapper.h:1547
karto::LinkInfo::LinkInfo
LinkInfo()
Definition: Mapper.h:150
karto::Vector2::GetX
const T & GetX() const
Definition: Karto.h:1028
karto::Matrix3::Transpose
Matrix3 Transpose() const
Definition: Karto.h:2527
karto::LocalizedRangeScan
Definition: Karto.h:5505
karto::ScanSolver::~ScanSolver
virtual ~ScanSolver()
Definition: Mapper.h:965
karto::LinkInfo::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:232
karto::Matrix3::FromAxisAngle
void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
Definition: Karto.h:2492
karto::MapperSensorManager::m_Scans
std::map< int, LocalizedRangeScan * > m_Scans
Definition: Mapper.h:1753
karto::Mapper::TryCloseLoop
kt_bool TryCloseLoop(LocalizedRangeScan *pScan, const Name &rSensorName)
Definition: Mapper.h:2066
karto::LinkInfo::GetCovariance
const Matrix3 & GetCovariance()
Definition: Mapper.h:219
karto::MapperLoopClosureListener::EndLoopClosure
virtual void EndLoopClosure(const std::string &)
Definition: Mapper.h:96
karto::ScanMatcher::m_pSearchSpaceProbs
Grid< kt_double > * m_pSearchSpaceProbs
Definition: Mapper.h:1477
karto::Mapper::m_pScanBufferMaximumScanDistance
Parameter< kt_double > * m_pScanBufferMaximumScanDistance
Definition: Mapper.h:2220
karto::Edge::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:478
karto::Mapper::m_pMinimumTimeInterval
Parameter< kt_double > * m_pMinimumTimeInterval
Definition: Mapper.h:2183
kt_int32s
int32_t kt_int32s
Definition: Types.h:51
karto::ScanMatcher::m_searchAngleResolution
kt_double m_searchAngleResolution
Definition: Mapper.h:1485
karto::Grid::GridIndex
virtual kt_int32s GridIndex(const Vector2< kt_int32s > &rGrid, kt_bool boundaryCheck=true) const
Definition: Karto.h:4757
karto::Graph::~Graph
virtual ~Graph()
Definition: Mapper.h:580
karto::Graph::VertexMap
std::map< Name, std::map< int, Vertex< T > * > > VertexMap
Definition: Mapper.h:567
karto::CorrelationGrid::m_Roi
Rectangle2< kt_int32s > m_Roi
Definition: Mapper.h:1287
karto::Matrix3
Definition: Karto.h:2444
karto::ScanSolver::IdPoseVector
std::vector< std::pair< kt_int32s, Pose2 > > IdPoseVector
Definition: Mapper.h:953
karto::Grid
Definition: Karto.h:4642
karto::ScanMatcher::m_nAngles
kt_int32u m_nAngles
Definition: Mapper.h:1484
karto::MapperGraph::GetVertex
Vertex< LocalizedRangeScan > * GetVertex(LocalizedRangeScan *pScan)
Definition: Mapper.h:823
karto::MapperLoopClosureListener::LoopClosureCheck
virtual void LoopClosureCheck(const std::string &)
Definition: Mapper.h:86
karto::MapperSensorManager
Definition: Mapper.h:1539
karto::Vertex::GetObject
T * GetObject() const
Definition: Mapper.h:319
Karto.h
karto::EdgeLabel::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:129
karto::Mapper::m_pLoopMatchMaximumVarianceCoarse
Parameter< kt_double > * m_pLoopMatchMaximumVarianceCoarse
Definition: Mapper.h:2261
karto::LinkInfo::access
friend class boost::serialization::access
Definition: Mapper.h:230
karto::MapperGraph::m_pTraversal
GraphTraversal< LocalizedRangeScan > * m_pTraversal
Definition: Mapper.h:919
karto::MapperSensorManager::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:1733
karto::MapperListener
Definition: Mapper.h:46
karto::LinkInfo
Definition: Mapper.h:141
karto::CorrelationGrid::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:1293
karto::ScanMatcher::m_searchAngleOffset
kt_double m_searchAngleOffset
Definition: Mapper.h:1483
karto::ScanMatcher::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:1493
karto::SensorData::GetStateId
kt_int32s GetStateId() const
Definition: Karto.h:5145
karto::ScanMatcher::GetCorrelationGrid
CorrelationGrid * GetCorrelationGrid() const
Definition: Mapper.h:1422
karto::MapperGraph
Definition: Mapper.h:713
karto::Vector2::GetY
const T & GetY() const
Definition: Karto.h:1046
karto::GraphTraversal::GraphTraversal
GraphTraversal(Graph< T > *pGraph)
Definition: Mapper.h:527
karto::ScanMatcher::m_pCorrelationGrid
CorrelationGrid * m_pCorrelationGrid
Definition: Mapper.h:1476
karto::MapperSensorManager::m_ScanManagers
ScanManagerMap m_ScanManagers
Definition: Mapper.h:1746
karto::Vertex::m_pObject
T * m_pObject
Definition: Mapper.h:374
karto::Mapper::m_pMapperSensorManager
MapperSensorManager * m_pMapperSensorManager
Definition: Mapper.h:2148
KARTO_EXPORT
#define KARTO_EXPORT
Definition: Macros.h:56
karto::Graph::m_Vertices
VertexMap m_Vertices
Definition: Mapper.h:683
karto::MapperLoopClosureListener
Definition: Mapper.h:80
karto::LocalizedRangeScanVector
std::vector< LocalizedRangeScan * > LocalizedRangeScanVector
Definition: Karto.h:5879
karto::MapperSensorManager::GetSensorNames
std::vector< Name > GetSensorNames()
Definition: Mapper.h:1585
karto::CorrelationGrid::CorrelationGrid
CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize, kt_double resolution, kt_double smearDeviation)
Definition: Mapper.h:1184
karto::Parameter< kt_bool >
karto::CorrelationGrid::CreateGrid
static CorrelationGrid * CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution, kt_double smearDeviation)
Definition: Mapper.h:1088
karto::Mapper::m_pMinimumTravelDistance
Parameter< kt_double > * m_pMinimumTravelDistance
Definition: Mapper.h:2193
karto::Graph::GetVertices
const VertexMap & GetVertices() const
Definition: Mapper.h:674
karto::ScanSolver::RemoveNode
virtual void RemoveNode(kt_int32s)
Definition: Mapper.h:991
karto::ScanSolver::Reset
virtual void Reset()
Definition: Mapper.h:1019
karto::Vertex::RemoveObject
void RemoveObject()
Definition: Mapper.h:327
karto::Vertex::GetAdjacentVertices
std::vector< Vertex< T > * > GetAdjacentVertices() const
Definition: Mapper.h:336
karto::GraphTraversal::~GraphTraversal
virtual ~GraphTraversal()
Definition: Mapper.h:532
karto::Mapper::m_Deserialized
kt_bool m_Deserialized
Definition: Mapper.h:2144
karto::Mapper::m_pLoopSearchSpaceDimension
Parameter< kt_double > * m_pLoopSearchSpaceDimension
Definition: Mapper.h:2304
karto::LocalizationScanVertex::LocalizationScanVertex
LocalizationScanVertex()
Definition: Mapper.h:1920
karto::MapperListener::Info
virtual void Info(const std::string &)
Definition: Mapper.h:52
karto::Edge::GetLabel
EdgeLabel * GetLabel()
Definition: Mapper.h:457
karto::GraphTraversal
Definition: Mapper.h:521
karto::Graph::RemoveEdge
void RemoveEdge(const int &idx)
Definition: Mapper.h:629
karto::Graph::AddEdge
void AddEdge(Edge< T > *pEdge)
Definition: Mapper.h:620
karto::MapperSensorManager::m_RunningBufferMaximumSize
kt_int32u m_RunningBufferMaximumSize
Definition: Mapper.h:1748
karto::Mapper::m_pLinkScanMaximumDistance
Parameter< kt_double > * m_pLinkScanMaximumDistance
Definition: Mapper.h:2233
karto::CorrelationGrid::m_KernelSize
kt_int32s m_KernelSize
Definition: Mapper.h:1281
karto::MapperListener::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:55
karto::Mapper::m_pLoopMatchMinimumChainSize
Parameter< kt_int32u > * m_pLoopMatchMinimumChainSize
Definition: Mapper.h:2254
karto::Vertex::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:380
karto::CorrelationGrid::CalculateKernel
virtual void CalculateKernel()
Definition: Mapper.h:1202
karto::Mapper::m_pLoopSearchSpaceSmearDeviation
Parameter< kt_double > * m_pLoopSearchSpaceSmearDeviation
Definition: Mapper.h:2316
karto::Vertex::SetScore
void SetScore(const double score)
Definition: Mapper.h:310
karto::ScanMatcher::m_pPoseResponse
std::pair< kt_double, Pose2 > * m_pPoseResponse
Definition: Mapper.h:1479
karto::Mapper::m_pGraph
MapperGraph * m_pGraph
Definition: Mapper.h:2150
karto::GraphTraversal::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:546
karto::ScanManager
Definition: Mapper.cpp:57
nanoflann_adaptors.h
kt_bool
bool kt_bool
Definition: Types.h:64
karto::Vertex::~Vertex
virtual ~Vertex()
Definition: Mapper.h:274
karto::Graph::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:694
karto::GraphTraversal::TraverseForScans
virtual std::vector< T * > TraverseForScans(Vertex< T > *pStartVertex, Visitor< T > *pVisitor)=0
karto::Mapper::m_pFineSearchAngleOffset
Parameter< kt_double > * m_pFineSearchAngleOffset
Definition: Mapper.h:2328
karto::ScanMatcher::m_xPoses
std::vector< kt_double > m_xPoses
Definition: Mapper.h:1480
karto::Mapper::GetMapperSensorManager
MapperSensorManager * GetMapperSensorManager() const
Definition: Mapper.h:2056
karto::CorrelationGrid::CorrelationGrid
CorrelationGrid()
Definition: Mapper.h:1085
karto::LinkInfo::m_Covariance
Matrix3 m_Covariance
Definition: Mapper.h:228
karto::EdgeLabel::~EdgeLabel
virtual ~EdgeLabel()
Definition: Mapper.h:124
karto::Edge::m_pLabel
EdgeLabel * m_pLabel
Definition: Mapper.h:474
karto::ScanMatcher::m_yPoses
std::vector< kt_double > m_yPoses
Definition: Mapper.h:1481
karto::MapperSensorManager::ScanManagerMap
std::map< Name, ScanManager * > ScanManagerMap
Definition: Mapper.h:1541
karto::Graph::GetEdges
const std::vector< Edge< T > * > & GetEdges() const
Definition: Mapper.h:665
karto::math::InRange
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:172
karto::Mapper::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:2344
karto::MapperSensorManager::m_NextScanId
kt_int32s m_NextScanId
Definition: Mapper.h:1751
karto::PointVectorDouble
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1283
karto::MapperSensorManager::GetScanManager
ScanManager * GetScanManager(const Name &rSensorName)
Definition: Mapper.h:1721
karto::LinkInfo::m_Pose1
Pose2 m_Pose1
Definition: Mapper.h:225
karto::LinkInfo::LinkInfo
LinkInfo(const Pose2 &rPose1, const Pose2 &rPose2, const Matrix3 &rCovariance)
Definition: Mapper.h:153
karto::LocalizationScanVertex::vertex
Vertex< LocalizedRangeScan > * vertex
Definition: Mapper.h:1923
karto::Vector2< kt_int32s >
karto::ScanSolver::GetCorrections
virtual const IdPoseVector & GetCorrections() const =0
karto::Vertex::GetScore
const double GetScore() const
Definition: Mapper.h:301
karto::Mapper::m_pLinkMatchMinimumResponseFine
Parameter< kt_double > * m_pLinkMatchMinimumResponseFine
Definition: Mapper.h:2226
karto::MapperLoopClosureListener::BeginLoopClosure
virtual void BeginLoopClosure(const std::string &)
Definition: Mapper.h:91
karto::Mapper::m_pSequentialScanMatcher
ScanMatcher * m_pSequentialScanMatcher
Definition: Mapper.h:2146
forEachAs
#define forEachAs(listtype, list, iter)
Definition: Macros.h:69
karto::CorrelationGrid::GetROI
const Rectangle2< kt_int32s > & GetROI() const
Definition: Mapper.h:1121
karto::ScanMatcher::ScanMatcher
ScanMatcher()
Definition: Mapper.h:1317
karto::LinkInfo::m_PoseDifference
Pose2 m_PoseDifference
Definition: Mapper.h:227
karto::Mapper::m_pCorrelationSearchSpaceResolution
Parameter< kt_double > * m_pCorrelationSearchSpaceResolution
Definition: Mapper.h:2288
karto::Mapper::m_pCoarseSearchAngleOffset
Parameter< kt_double > * m_pCoarseSearchAngleOffset
Definition: Mapper.h:2329
karto::LinkInfo::m_Pose2
Pose2 m_Pose2
Definition: Mapper.h:226
karto::MapperSensorManager::GetScan
LocalizedRangeScan * GetScan(kt_int32s id)
Definition: Mapper.h:1626
karto::Edge::GetTarget
Vertex< T > * GetTarget() const
Definition: Mapper.h:448
karto::Mapper::m_pMinimumAnglePenalty
Parameter< kt_double > * m_pMinimumAnglePenalty
Definition: Mapper.h:2336
karto::Transform
Definition: Karto.h:2976
karto::LinkInfo::GetPose2
const Pose2 & GetPose2()
Definition: Mapper.h:201
karto::MapperSensorManager::MapperSensorManager
MapperSensorManager()
Definition: Mapper.h:1554
karto::Edge::Edge
Edge()
Definition: Mapper.h:404
karto::Edge::SetLabel
void SetLabel(EdgeLabel *pLabel)
Definition: Mapper.h:466
karto::Mapper::m_pScanBufferSize
Parameter< kt_int32u > * m_pScanBufferSize
Definition: Mapper.h:2213
karto::MapperDebugListener::Debug
virtual void Debug(const std::string &)
Definition: Mapper.h:69
karto::ScanMatcher::m_pMapper
Mapper * m_pMapper
Definition: Mapper.h:1475
karto::Mapper::m_pCoarseAngleResolution
Parameter< kt_double > * m_pCoarseAngleResolution
Definition: Mapper.h:2332
karto::Visitor::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:505
karto::CorrelationGrid::~CorrelationGrid
virtual ~CorrelationGrid()
Definition: Mapper.h:1067
karto::ScanMatcher::m_doPenalize
kt_bool m_doPenalize
Definition: Mapper.h:1486
karto::ScanSolver::GetNodeOrientation
virtual void GetNodeOrientation(const int &unique_id, double &pose)
Definition: Mapper.h:1042
karto::LocalizationScanVertex
Definition: Mapper.h:1918
kt_int32u
uint32_t kt_int32u
Definition: Types.h:52
karto::MapperSensorManager::~MapperSensorManager
virtual ~MapperSensorManager()
Definition: Mapper.h:1560
karto::Mapper::m_pUseScanBarycenter
Parameter< kt_bool > * m_pUseScanBarycenter
Definition: Mapper.h:2170
karto::Mapper::m_pUseScanMatching
Parameter< kt_bool > * m_pUseScanMatching
Definition: Mapper.h:2165
karto::Edge::m_pTarget
Vertex< T > * m_pTarget
Definition: Mapper.h:473
karto::Mapper::m_pLoopMatchMinimumResponseCoarse
Parameter< kt_double > * m_pLoopMatchMinimumResponseCoarse
Definition: Mapper.h:2267
karto::CorrelationGrid::SmearPoint
void SmearPoint(const Vector2< kt_int32s > &rGridPoint)
Definition: Mapper.h:1139
karto::Vertex
Definition: Mapper.h:253
karto::MapperGraph::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:926
karto::Mapper::m_pDistanceVariancePenalty
Parameter< kt_double > * m_pDistanceVariancePenalty
Definition: Mapper.h:2324
karto::Mapper::m_pCorrelationSearchSpaceSmearDeviation
Parameter< kt_double > * m_pCorrelationSearchSpaceSmearDeviation
Definition: Mapper.h:2294
karto::EdgeLabel::EdgeLabel
EdgeLabel()
Definition: Mapper.h:117
karto::GridIndexLookup< kt_int8u >
karto::Mapper::m_pLoopMatchMinimumResponseFine
Parameter< kt_double > * m_pLoopMatchMinimumResponseFine
Definition: Mapper.h:2273
karto::Mapper::m_pMinimumTravelHeading
Parameter< kt_double > * m_pMinimumTravelHeading
Definition: Mapper.h:2203
karto::LocalizationScanVertices
std::queue< LocalizationScanVertex > LocalizationScanVertices
Definition: Mapper.h:1926
karto::MapperLoopClosureListener::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:99
karto::MapperSensorManager::m_RunningBufferMaximumDistance
kt_double m_RunningBufferMaximumDistance
Definition: Mapper.h:1749
karto::GraphTraversal::m_pGraph
Graph< T > * m_pGraph
Definition: Mapper.h:542
karto::Vertex::RemoveEdge
void RemoveEdge(const int &idx)
Definition: Mapper.h:290
karto::LinkInfo::~LinkInfo
virtual ~LinkInfo()
Definition: Mapper.h:161
karto::GraphTraversal::GraphTraversal
GraphTraversal()
Definition: Mapper.h:524
process_constraints.filename
filename
Definition: process_constraints.py:114
karto::Mapper::SetUseScanMatching
void SetUseScanMatching(kt_bool val)
Definition: Mapper.h:2140
karto::Graph::Graph
Graph()
Definition: Mapper.h:573
karto::Edge::m_pSource
Vertex< T > * m_pSource
Definition: Mapper.h:472
forEach
#define forEach(listtype, list)
Definition: Macros.h:66
karto::Pose2
Definition: Karto.h:2046
karto::Object
Definition: Karto.h:634
karto::Mapper::m_pMinimumDistancePenalty
Parameter< kt_double > * m_pMinimumDistancePenalty
Definition: Mapper.h:2337
karto::Graph::AddVertex
void AddVertex(const Name &rName, Vertex< T > *pVertex)
Definition: Mapper.h:591
karto::ScanSolver::ScanSolver
ScanSolver()
Definition: Mapper.h:958
karto::Mapper::m_Initialized
kt_bool m_Initialized
Definition: Mapper.h:2143
karto::EdgeLabel
Definition: Mapper.h:111
karto::Mapper::m_pLoopSearchSpaceResolution
Parameter< kt_double > * m_pLoopSearchSpaceResolution
Definition: Mapper.h:2310
karto::ScanMatcher
Definition: Mapper.h:1314
karto::math::IsUpTo
kt_bool IsUpTo(const T &value, const T &maximum)
Definition: Math.h:147
karto::GridStates_Occupied
@ GridStates_Occupied
Definition: Karto.h:4447
karto::Graph::Clear
void Clear()
Definition: Mapper.h:639
karto::CorrelationGrid
Definition: Mapper.h:1061
karto::Mapper::m_pScanOptimizer
ScanSolver * m_pScanOptimizer
Definition: Mapper.h:2151
karto::MapperGraph::m_pLoopScanMatcher
ScanMatcher * m_pLoopScanMatcher
Definition: Mapper.h:914
kt_int8u
uint8_t kt_int8u
Definition: Types.h:46
karto::Edge::~Edge
virtual ~Edge()
Definition: Mapper.h:422
karto::Mapper::m_LocalizationScanVertices
LocalizationScanVertices m_LocalizationScanVertices
Definition: Mapper.h:2152
karto::Mapper::m_Listeners
std::vector< MapperListener * > m_Listeners
Definition: Mapper.h:2155
karto::MapperDebugListener::serialize
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.h:72
karto::CorrelationGrid::m_pKernel
kt_int8u * m_pKernel
Definition: Mapper.h:1284
karto::ScanSolver::getGraph
virtual std::unordered_map< int, Eigen::Vector3d > * getGraph()
Definition: Mapper.h:1026
karto::ScanSolver::Clear
virtual void Clear()
Definition: Mapper.h:1012
karto::CorrelationGrid::m_SmearDeviation
kt_double m_SmearDeviation
Definition: Mapper.h:1278
karto::Edge::GetSource
Vertex< T > * GetSource() const
Definition: Mapper.h:439
karto::Vertex::GetEdges
const std::vector< Edge< T > * > & GetEdges() const
Definition: Mapper.h:282
karto::GraphTraversal::TraverseForVertices
virtual std::vector< Vertex< T > * > TraverseForVertices(Vertex< T > *pStartVertex, Visitor< T > *pVisitor)=0
karto::MapperSensorManager::GetScanManager
ScanManager * GetScanManager(LocalizedRangeScan *pScan)
Definition: Mapper.h:1711
karto::ScanSolver::AddNode
virtual void AddNode(Vertex< LocalizedRangeScan > *)
Definition: Mapper.h:984
karto::Graph::RemoveVertex
void RemoveVertex(const Name &rName, const int &idx)
Definition: Mapper.h:601
karto::CorrelationGrid::GetHalfKernelSize
static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution)
Definition: Mapper.h:1266
karto::Mapper::m_pUseResponseExpansion
Parameter< kt_bool > * m_pUseResponseExpansion
Definition: Mapper.h:2340
karto::Edge
Definition: Mapper.h:247
karto::CorrelationGrid::GridIndex
virtual kt_int32s GridIndex(const Vector2< kt_int32s > &rGrid, kt_bool boundaryCheck=true) const
Definition: Mapper.h:1109
karto::Pose2::GetHeading
kt_double GetHeading() const
Definition: Karto.h:2153
karto::LocalizedRangeScanMap
std::map< int, LocalizedRangeScan * > LocalizedRangeScanMap
Definition: Karto.h:5880
karto::ScanMatcher::ScanMatcher
ScanMatcher(Mapper *pMapper)
Definition: Mapper.h:1464
karto::Vertex::Vertex
Vertex(T *pObject)
Definition: Mapper.h:266
karto
Definition: Karto.h:88
karto::Mapper::m_pCorrelationSearchSpaceDimension
Parameter< kt_double > * m_pCorrelationSearchSpaceDimension
Definition: Mapper.h:2282
karto::SensorData::GetSensorName
const Name & GetSensorName() const
Definition: Karto.h:5199
karto::Graph
Definition: Mapper.h:515
karto::LinkInfo::GetPoseDifference
const Pose2 & GetPoseDifference()
Definition: Mapper.h:210


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