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 OPEN_KARTO_MAPPER_H
19 #define OPEN_KARTO_MAPPER_H
20 
21 #include <map>
22 #include <vector>
23 
24 #include <open_karto/Karto.h>
25 
26 namespace karto
27 {
29  // Listener classes
30 
35  {
36  public:
40  virtual void Info(const std::string& /*rInfo*/) {};
41  };
42 
47  {
48  public:
52  virtual void Debug(const std::string& /*rInfo*/) {};
53  };
54 
59  {
60  public:
64  virtual void LoopClosureCheck(const std::string& /*rInfo*/) {};
65 
69  virtual void BeginLoopClosure(const std::string& /*rInfo*/) {};
70 
74  virtual void EndLoopClosure(const std::string& /*rInfo*/) {};
75  }; // MapperListener
76 
80 
84  class EdgeLabel
85  {
86  public:
91  {
92  }
93 
97  virtual ~EdgeLabel()
98  {
99  }
100  }; // EdgeLabel
101 
105 
106  // A LinkInfo object contains the requisite information for the "spring"
107  // that links two scans together--the pose difference and the uncertainty
108  // (represented by a covariance matrix).
109  class LinkInfo : public EdgeLabel
110  {
111  public:
118  LinkInfo(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
119  {
120  Update(rPose1, rPose2, rCovariance);
121  }
122 
126  virtual ~LinkInfo()
127  {
128  }
129 
130  public:
137  void Update(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
138  {
139  m_Pose1 = rPose1;
140  m_Pose2 = rPose2;
141 
142  // transform second pose into the coordinate system of the first pose
143  Transform transform(rPose1, Pose2());
144  m_PoseDifference = transform.TransformPose(rPose2);
145 
146  // transform covariance into reference of first pose
147  Matrix3 rotationMatrix;
148  rotationMatrix.FromAxisAngle(0, 0, 1, -rPose1.GetHeading());
149 
150  m_Covariance = rotationMatrix * rCovariance * rotationMatrix.Transpose();
151  }
152 
157  inline const Pose2& GetPose1()
158  {
159  return m_Pose1;
160  }
161 
166  inline const Pose2& GetPose2()
167  {
168  return m_Pose2;
169  }
170 
175  inline const Pose2& GetPoseDifference()
176  {
177  return m_PoseDifference;
178  }
179 
184  inline const Matrix3& GetCovariance()
185  {
186  return m_Covariance;
187  }
188 
189  private:
194  }; // LinkInfo
195 
199 
200  template<typename T>
201  class Edge;
202 
206  template<typename T>
207  class Vertex
208  {
209  friend class Edge<T>;
210 
211  public:
216  Vertex(T* pObject)
217  : m_pObject(pObject)
218  {
219  }
220 
224  virtual ~Vertex()
225  {
226  }
227 
232  inline const std::vector<Edge<T>*>& GetEdges() const
233  {
234  return m_Edges;
235  }
236 
241  inline T* GetObject() const
242  {
243  return m_pObject;
244  }
245 
250  std::vector<Vertex<T>*> GetAdjacentVertices() const
251  {
252  std::vector<Vertex<T>*> vertices;
253 
254  const_forEach(typename std::vector<Edge<T>*>, &m_Edges)
255  {
256  Edge<T>* pEdge = *iter;
257 
258  // check both source and target because we have a undirected graph
259  if (pEdge->GetSource() != this)
260  {
261  vertices.push_back(pEdge->GetSource());
262  }
263 
264  if (pEdge->GetTarget() != this)
265  {
266  vertices.push_back(pEdge->GetTarget());
267  }
268  }
269 
270  return vertices;
271  }
272 
273  private:
278  inline void AddEdge(Edge<T>* pEdge)
279  {
280  m_Edges.push_back(pEdge);
281  }
282 
284  std::vector<Edge<T>*> m_Edges;
285  }; // Vertex<T>
286 
290 
294  template<typename T>
295  class Edge
296  {
297  public:
303  Edge(Vertex<T>* pSource, Vertex<T>* pTarget)
304  : m_pSource(pSource)
305  , m_pTarget(pTarget)
306  , m_pLabel(NULL)
307  {
308  m_pSource->AddEdge(this);
309  m_pTarget->AddEdge(this);
310  }
311 
315  virtual ~Edge()
316  {
317  m_pSource = NULL;
318  m_pTarget = NULL;
319 
320  if (m_pLabel != NULL)
321  {
322  delete m_pLabel;
323  m_pLabel = NULL;
324  }
325  }
326 
327  public:
332  inline Vertex<T>* GetSource() const
333  {
334  return m_pSource;
335  }
336 
341  inline Vertex<T>* GetTarget() const
342  {
343  return m_pTarget;
344  }
345 
350  inline EdgeLabel* GetLabel()
351  {
352  return m_pLabel;
353  }
354 
359  inline void SetLabel(EdgeLabel* pLabel)
360  {
361  m_pLabel = pLabel;
362  }
363 
364  private:
368  }; // class Edge<T>
369 
373 
377  template<typename T>
378  class Visitor
379  {
380  public:
386  virtual kt_bool Visit(Vertex<T>* pVertex) = 0;
387  }; // Visitor<T>
388 
392 
393  template<typename T>
394  class Graph;
395 
399  template<typename T>
401  {
402  public:
408  : m_pGraph(pGraph)
409  {
410  }
411 
415  virtual ~GraphTraversal()
416  {
417  }
418 
419  public:
425  virtual std::vector<T*> Traverse(Vertex<T>* pStartVertex, Visitor<T>* pVisitor) = 0;
426 
427  protected:
432  }; // GraphTraversal<T>
433 
437 
441  template<typename T>
442  class Graph
443  {
444  public:
448  typedef std::map<Name, std::vector<Vertex<T>*> > VertexMap;
449 
450  public:
455  {
456  }
457 
461  virtual ~Graph()
462  {
463  Clear();
464  }
465 
466  public:
472  inline void AddVertex(const Name& rName, Vertex<T>* pVertex)
473  {
474  m_Vertices[rName].push_back(pVertex);
475  }
476 
481  inline void AddEdge(Edge<T>* pEdge)
482  {
483  m_Edges.push_back(pEdge);
484  }
485 
489  void Clear()
490  {
491  forEachAs(typename VertexMap, &m_Vertices, indexIter)
492  {
493  // delete each vertex
494  forEach(typename std::vector<Vertex<T>*>, &(indexIter->second))
495  {
496  delete *iter;
497  }
498  }
499  m_Vertices.clear();
500 
501  const_forEach(typename std::vector<Edge<T>*>, &m_Edges)
502  {
503  delete *iter;
504  }
505  m_Edges.clear();
506  }
507 
512  inline const std::vector<Edge<T>*>& GetEdges() const
513  {
514  return m_Edges;
515  }
516 
521  inline const VertexMap& GetVertices() const
522  {
523  return m_Vertices;
524  }
525 
526  protected:
530  VertexMap m_Vertices;
531 
535  std::vector<Edge<T>*> m_Edges;
536  }; // Graph<T>
537 
541 
542  class Mapper;
543  class ScanMatcher;
544 
548  class KARTO_EXPORT MapperGraph : public Graph<LocalizedRangeScan>
549  {
550  public:
556  MapperGraph(Mapper* pMapper, kt_double rangeThreshold);
557 
561  virtual ~MapperGraph();
562 
563  public:
568  void AddVertex(LocalizedRangeScan* pScan);
569 
578  Edge<LocalizedRangeScan>* AddEdge(LocalizedRangeScan* pSourceScan,
579  LocalizedRangeScan* pTargetScan,
580  kt_bool& rIsNewEdge);
581 
587  void AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance);
588 
594  kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName);
595 
601  LocalizedRangeScanVector FindNearLinkedScans(LocalizedRangeScan* pScan, kt_double maxDistance);
602 
608  {
609  return m_pLoopScanMatcher;
610  }
611 
612  private:
619  {
620  return m_Vertices[pScan->GetSensorName()][pScan->GetStateId()];
621  }
622 
628  LocalizedRangeScan* GetClosestScanToPose(const LocalizedRangeScanVector& rScans, const Pose2& rPose) const;
629 
637  void LinkScans(LocalizedRangeScan* pFromScan,
638  LocalizedRangeScan* pToScan,
639  const Pose2& rMean,
640  const Matrix3& rCovariance);
641 
648  void LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector<Matrix3>& rCovariances);
649 
657  void LinkChainToScan(const LocalizedRangeScanVector& rChain,
658  LocalizedRangeScan* pScan,
659  const Pose2& rMean,
660  const Matrix3& rCovariance);
661 
667  std::vector<LocalizedRangeScanVector> FindNearChains(LocalizedRangeScan* pScan);
668 
675  Pose2 ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector<Matrix3>& rCovariances) const;
676 
685  LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan* pScan,
686  const Name& rSensorName,
687  kt_int32u& rStartNum);
688 
692  void CorrectPoses();
693 
694  private:
699 
704 
709  }; // MapperGraph
710 
714 
719  {
720  public:
724  typedef std::vector<std::pair<kt_int32s, Pose2> > IdPoseVector;
725 
730  {
731  }
732 
736  virtual ~ScanSolver()
737  {
738  }
739 
740  public:
744  virtual void Compute() = 0;
745 
750  virtual const IdPoseVector& GetCorrections() const = 0;
751 
755  virtual void AddNode(Vertex<LocalizedRangeScan>* /*pVertex*/)
756  {
757  }
758 
762  virtual void RemoveNode(kt_int32s /*id*/)
763  {
764  }
765 
769  virtual void AddConstraint(Edge<LocalizedRangeScan>* /*pEdge*/)
770  {
771  }
772 
776  virtual void RemoveConstraint(kt_int32s /*sourceId*/, kt_int32s /*targetId*/)
777  {
778  }
779 
783  virtual void Clear() {};
784  }; // ScanSolver
785 
789 
793  class CorrelationGrid : public Grid<kt_int8u>
794  {
795  public:
800  {
801  delete [] m_pKernel;
802  }
803 
804  public:
814  kt_int32s height,
815  kt_double resolution,
816  kt_double smearDeviation)
817  {
818  assert(resolution != 0.0);
819 
820  // +1 in case of roundoff
821  kt_int32u borderSize = GetHalfKernelSize(smearDeviation, resolution) + 1;
822 
823  CorrelationGrid* pGrid = new CorrelationGrid(width, height, borderSize, resolution, smearDeviation);
824 
825  return pGrid;
826  }
827 
834  virtual kt_int32s GridIndex(const Vector2<kt_int32s>& rGrid, kt_bool boundaryCheck = true) const
835  {
836  kt_int32s x = rGrid.GetX() + m_Roi.GetX();
837  kt_int32s y = rGrid.GetY() + m_Roi.GetY();
838 
839  return Grid<kt_int8u>::GridIndex(Vector2<kt_int32s>(x, y), boundaryCheck);
840  }
841 
846  inline const Rectangle2<kt_int32s>& GetROI() const
847  {
848  return m_Roi;
849  }
850 
855  inline void SetROI(const Rectangle2<kt_int32s>& roi)
856  {
857  m_Roi = roi;
858  }
859 
864  inline void SmearPoint(const Vector2<kt_int32s>& rGridPoint)
865  {
866  assert(m_pKernel != NULL);
867 
868  int gridIndex = GridIndex(rGridPoint);
869  if (GetDataPointer()[gridIndex] != GridStates_Occupied)
870  {
871  return;
872  }
873 
874  kt_int32s halfKernel = m_KernelSize / 2;
875 
876  // apply kernel
877  for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
878  {
879  kt_int8u* pGridAdr = GetDataPointer(Vector2<kt_int32s>(rGridPoint.GetX(), rGridPoint.GetY() + j));
880 
881  kt_int32s kernelConstant = (halfKernel) + m_KernelSize * (j + halfKernel);
882 
883  // if a point is on the edge of the grid, there is no problem
884  // with running over the edge of allowable memory, because
885  // the grid has margins to compensate for the kernel size
886  for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
887  {
888  kt_int32s kernelArrayIndex = i + kernelConstant;
889 
890  kt_int8u kernelValue = m_pKernel[kernelArrayIndex];
891  if (kernelValue > pGridAdr[i])
892  {
893  // kernel value is greater, so set it to kernel value
894  pGridAdr[i] = kernelValue;
895  }
896  }
897  }
898  }
899 
900  protected:
909  CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize,
910  kt_double resolution, kt_double smearDeviation)
911  : Grid<kt_int8u>(width + borderSize * 2, height + borderSize * 2)
912  , m_SmearDeviation(smearDeviation)
913  , m_pKernel(NULL)
914  {
915  GetCoordinateConverter()->SetScale(1.0 / resolution);
916 
917  // setup region of interest
918  m_Roi = Rectangle2<kt_int32s>(borderSize, borderSize, width, height);
919 
920  // calculate kernel
921  CalculateKernel();
922  }
923 
927  virtual void CalculateKernel()
928  {
929  kt_double resolution = GetResolution();
930 
931  assert(resolution != 0.0);
932  assert(m_SmearDeviation != 0.0);
933 
934  // min and max distance deviation for smearing;
935  // will smear for two standard deviations, so deviation must be at least 1/2 of the resolution
936  const kt_double MIN_SMEAR_DISTANCE_DEVIATION = 0.5 * resolution;
937  const kt_double MAX_SMEAR_DISTANCE_DEVIATION = 10 * resolution;
938 
939  // check if given value too small or too big
940  if (!math::InRange(m_SmearDeviation, MIN_SMEAR_DISTANCE_DEVIATION, MAX_SMEAR_DISTANCE_DEVIATION))
941  {
942  std::stringstream error;
943  error << "Mapper Error: Smear deviation too small: Must be between "
944  << MIN_SMEAR_DISTANCE_DEVIATION
945  << " and "
946  << MAX_SMEAR_DISTANCE_DEVIATION;
947  throw std::runtime_error(error.str());
948  }
949 
950  // NOTE: Currently assumes a two-dimensional kernel
951 
952  // +1 for center
953  m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1;
954 
955  // allocate kernel
956  m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];
957  if (m_pKernel == NULL)
958  {
959  throw std::runtime_error("Unable to allocate memory for kernel!");
960  }
961 
962  // calculate kernel
963  kt_int32s halfKernel = m_KernelSize / 2;
964  for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
965  {
966  for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
967  {
968 #ifdef WIN32
969  kt_double distanceFromMean = _hypot(i * resolution, j * resolution);
970 #else
971  kt_double distanceFromMean = hypot(i * resolution, j * resolution);
972 #endif
973  kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2));
974 
975  kt_int32u kernelValue = static_cast<kt_int32u>(math::Round(z * GridStates_Occupied));
976  assert(math::IsUpTo(kernelValue, static_cast<kt_int32u>(255)));
977 
978  int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel);
979  m_pKernel[kernelArrayIndex] = static_cast<kt_int8u>(kernelValue);
980  }
981  }
982  }
983 
991  static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution)
992  {
993  assert(resolution != 0.0);
994 
995  return static_cast<kt_int32s>(math::Round(2.0 * smearDeviation / resolution));
996  }
997 
998  private:
1004 
1005  // Size of one side of the kernel
1007 
1008  // Cached kernel for smearing
1010 
1011  // region of interest
1013  }; // CorrelationGrid
1014 
1018 
1023  {
1024  public:
1028  virtual ~ScanMatcher();
1029 
1030  public:
1034  static ScanMatcher* Create(Mapper* pMapper,
1035  kt_double searchSize,
1036  kt_double resolution,
1037  kt_double smearDeviation,
1038  kt_double rangeThreshold);
1039 
1050  kt_double MatchScan(LocalizedRangeScan* pScan,
1051  const LocalizedRangeScanVector& rBaseScans,
1052  Pose2& rMean, Matrix3& rCovariance,
1053  kt_bool doPenalize = true,
1054  kt_bool doRefineMatch = true);
1055 
1072  kt_double CorrelateScan(LocalizedRangeScan* pScan,
1073  const Pose2& rSearchCenter,
1074  const Vector2<kt_double>& rSearchSpaceOffset,
1075  const Vector2<kt_double>& rSearchSpaceResolution,
1076  kt_double searchAngleOffset,
1077  kt_double searchAngleResolution,
1078  kt_bool doPenalize,
1079  Pose2& rMean,
1080  Matrix3& rCovariance,
1081  kt_bool doingFineMatch);
1082 
1093  void ComputePositionalCovariance(const Pose2& rBestPose,
1094  kt_double bestResponse,
1095  const Pose2& rSearchCenter,
1096  const Vector2<kt_double>& rSearchSpaceOffset,
1097  const Vector2<kt_double>& rSearchSpaceResolution,
1098  kt_double searchAngleResolution,
1099  Matrix3& rCovariance);
1100 
1110  void ComputeAngularCovariance(const Pose2& rBestPose,
1111  kt_double bestResponse,
1112  const Pose2& rSearchCenter,
1113  kt_double searchAngleOffset,
1114  kt_double searchAngleResolution,
1115  Matrix3& rCovariance);
1116 
1122  {
1123  return m_pCorrelationGrid;
1124  }
1125 
1126  private:
1132  void AddScans(const LocalizedRangeScanVector& rScans, Vector2<kt_double> viewPoint);
1133 
1140  void AddScan(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint, kt_bool doSmear = true);
1141 
1148  PointVectorDouble FindValidPoints(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint) const;
1149 
1156  kt_double GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const;
1157 
1158  protected:
1163  : m_pMapper(pMapper)
1164  , m_pCorrelationGrid(NULL)
1165  , m_pSearchSpaceProbs(NULL)
1166  , m_pGridLookup(NULL)
1167  {
1168  }
1169 
1170  private:
1172 
1175 
1177  }; // ScanMatcher
1178 
1182 
1183  class ScanManager;
1184 
1188  class KARTO_EXPORT MapperSensorManager // : public SensorManager
1189  {
1190  typedef std::map<Name, ScanManager*> ScanManagerMap;
1191 
1192  public:
1196  MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
1197  : m_RunningBufferMaximumSize(runningBufferMaximumSize)
1198  , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
1199  , m_NextScanId(0)
1200  {
1201  }
1202 
1207  {
1208  Clear();
1209  }
1210 
1211  public:
1217  void RegisterSensor(const Name& rSensorName);
1218 
1225  LocalizedRangeScan* GetScan(const Name& rSensorName, kt_int32s scanIndex);
1226 
1231  inline std::vector<Name> GetSensorNames()
1232  {
1233  std::vector<Name> deviceNames;
1234  const_forEach(ScanManagerMap, &m_ScanManagers)
1235  {
1236  deviceNames.push_back(iter->first);
1237  }
1238 
1239  return deviceNames;
1240  }
1241 
1247  LocalizedRangeScan* GetLastScan(const Name& rSensorName);
1248 
1253  inline void SetLastScan(LocalizedRangeScan* pScan);
1254 
1261  {
1262  assert(math::IsUpTo(id, (kt_int32s)m_Scans.size()));
1263 
1264  return m_Scans[id];
1265  }
1266 
1271  void AddScan(LocalizedRangeScan* pScan);
1272 
1277  void AddRunningScan(LocalizedRangeScan* pScan);
1278 
1284  LocalizedRangeScanVector& GetScans(const Name& rSensorName);
1285 
1291  LocalizedRangeScanVector& GetRunningScans(const Name& rSensorName);
1292 
1297  LocalizedRangeScanVector GetAllScans();
1298 
1302  void Clear();
1303 
1304  private:
1310  {
1311  return GetScanManager(pScan->GetSensorName());
1312  }
1313 
1319  inline ScanManager* GetScanManager(const Name& rSensorName)
1320  {
1321  if (m_ScanManagers.find(rSensorName) != m_ScanManagers.end())
1322  {
1323  return m_ScanManagers[rSensorName];
1324  }
1325 
1326  return NULL;
1327  }
1328 
1329  private:
1330  // map from device ID to scan data
1331  ScanManagerMap m_ScanManagers;
1332 
1335 
1337 
1338  std::vector<LocalizedRangeScan*> m_Scans;
1339  }; // MapperSensorManager
1340 
1344 
1502  class KARTO_EXPORT Mapper : public Module
1503  {
1504  friend class MapperGraph;
1505  friend class ScanMatcher;
1506 
1507  public:
1511  Mapper();
1512 
1517  Mapper(const std::string& rName);
1518 
1522  virtual ~Mapper();
1523 
1524  public:
1529  void Initialize(kt_double rangeThreshold);
1530 
1535  void Reset();
1536 
1548  virtual kt_bool Process(LocalizedRangeScan* pScan);
1549 
1553  virtual kt_bool Process(Object* pObject);
1554 
1561  virtual const LocalizedRangeScanVector GetAllProcessedScans() const;
1562 
1567  void AddListener(MapperListener* pListener);
1568 
1573  void RemoveListener(MapperListener* pListener);
1574 
1579  void SetScanSolver(ScanSolver* pSolver);
1580 
1585  virtual MapperGraph* GetGraph() const;
1586 
1591  virtual ScanMatcher* GetSequentialScanMatcher() const;
1592 
1597  virtual ScanMatcher* GetLoopScanMatcher() const;
1598 
1604  {
1605  return m_pMapperSensorManager;
1606  }
1607 
1613  inline kt_bool TryCloseLoop(LocalizedRangeScan* pScan, const Name& rSensorName)
1614  {
1615  return m_pGraph->TryCloseLoop(pScan, rSensorName);
1616  }
1617 
1618  private:
1619  void InitializeParameters();
1620 
1628  kt_bool HasMovedEnough(LocalizedRangeScan* pScan, LocalizedRangeScan* pLastScan) const;
1629 
1630  public:
1632  // fire information for listeners!!
1633 
1638  void FireInfo(const std::string& rInfo) const;
1639 
1644  void FireDebug(const std::string& rInfo) const;
1645 
1650  void FireLoopClosureCheck(const std::string& rInfo) const;
1651 
1656  void FireBeginLoopClosure(const std::string& rInfo) const;
1657 
1662  void FireEndLoopClosure(const std::string& rInfo) const;
1663 
1664  // FireRunningScansUpdated
1665 
1666  // FireCovarianceCalculated
1667 
1668  // FireLoopClosureChain
1669 
1670  private:
1674  Mapper(const Mapper&);
1675 
1679  const Mapper& operator=(const Mapper&);
1680 
1681  public:
1682  void SetUseScanMatching(kt_bool val) { m_pUseScanMatching->SetValue(val); }
1683 
1684  private:
1686 
1688 
1690 
1693 
1694  std::vector<MapperListener*> m_Listeners;
1695 
1696 
1706 
1711 
1724 
1734 
1744 
1754 
1761 
1767 
1774 
1780 
1787 
1795 
1802 
1808 
1814 
1816  // CorrelationParameters correlationParameters;
1817 
1823 
1829 
1835 
1836 
1838  // CorrelationParameters loopCorrelationParameters;
1839 
1845 
1851 
1857 
1859  // ScanMatcherParameters;
1860 
1861  // Variance of penalty for deviating from odometry when scan-matching.
1862  // The penalty is a multiplier (less than 1.0) is a function of the
1863  // delta of the scan position being tested and the odometric pose
1866 
1867  // The range of angles to search during a coarse search and a finer search
1870 
1871  // Resolution of angles to search during a coarse search
1873 
1874  // Minimum value of the penalty multiplier so scores do not
1875  // become too small
1878 
1879  // whether to increase the search space if no good matches are initially found
1881 
1882  public:
1883  /* Abstract methods for parameter setters and getters */
1884 
1885  /* Getters */
1886  // General Parameters
1887  bool getParamUseScanMatching();
1888  bool getParamUseScanBarycenter();
1889  double getParamMinimumTimeInterval();
1890  double getParamMinimumTravelDistance();
1891  double getParamMinimumTravelHeading();
1892  int getParamScanBufferSize();
1893  double getParamScanBufferMaximumScanDistance();
1894  double getParamLinkMatchMinimumResponseFine();
1895  double getParamLinkScanMaximumDistance();
1896  double getParamLoopSearchMaximumDistance();
1897  bool getParamDoLoopClosing();
1898  int getParamLoopMatchMinimumChainSize();
1899  double getParamLoopMatchMaximumVarianceCoarse();
1900  double getParamLoopMatchMinimumResponseCoarse();
1901  double getParamLoopMatchMinimumResponseFine();
1902 
1903  // Correlation Parameters - Correlation Parameters
1904  double getParamCorrelationSearchSpaceDimension();
1905  double getParamCorrelationSearchSpaceResolution();
1906  double getParamCorrelationSearchSpaceSmearDeviation();
1907 
1908  // Correlation Parameters - Loop Closure Parameters
1909  double getParamLoopSearchSpaceDimension();
1910  double getParamLoopSearchSpaceResolution();
1911  double getParamLoopSearchSpaceSmearDeviation();
1912 
1913  // Scan Matcher Parameters
1914  double getParamDistanceVariancePenalty();
1915  double getParamAngleVariancePenalty();
1916  double getParamFineSearchAngleOffset();
1917  double getParamCoarseSearchAngleOffset();
1918  double getParamCoarseAngleResolution();
1919  double getParamMinimumAnglePenalty();
1920  double getParamMinimumDistancePenalty();
1921  bool getParamUseResponseExpansion();
1922 
1923  /* Setters */
1924  // General Parameters
1925  void setParamUseScanMatching(bool b);
1926  void setParamUseScanBarycenter(bool b);
1927  void setParamMinimumTimeInterval(double d);
1928  void setParamMinimumTravelDistance(double d);
1929  void setParamMinimumTravelHeading(double d);
1930  void setParamScanBufferSize(int i);
1931  void setParamScanBufferMaximumScanDistance(double d);
1932  void setParamLinkMatchMinimumResponseFine(double d);
1933  void setParamLinkScanMaximumDistance(double d);
1934  void setParamLoopSearchMaximumDistance(double d);
1935  void setParamDoLoopClosing(bool b);
1936  void setParamLoopMatchMinimumChainSize(int i);
1937  void setParamLoopMatchMaximumVarianceCoarse(double d);
1938  void setParamLoopMatchMinimumResponseCoarse(double d);
1939  void setParamLoopMatchMinimumResponseFine(double d);
1940 
1941  // Correlation Parameters - Correlation Parameters
1942  void setParamCorrelationSearchSpaceDimension(double d);
1943  void setParamCorrelationSearchSpaceResolution(double d);
1944  void setParamCorrelationSearchSpaceSmearDeviation(double d);
1945 
1946  // Correlation Parameters - Loop Closure Parameters
1947  void setParamLoopSearchSpaceDimension(double d);
1948  void setParamLoopSearchSpaceResolution(double d);
1949  void setParamLoopSearchSpaceSmearDeviation(double d);
1950 
1951  // Scan Matcher Parameters
1952  void setParamDistanceVariancePenalty(double d);
1953  void setParamAngleVariancePenalty(double d);
1954  void setParamFineSearchAngleOffset(double d);
1955  void setParamCoarseSearchAngleOffset(double d);
1956  void setParamCoarseAngleResolution(double d);
1957  void setParamMinimumAnglePenalty(double d);
1958  void setParamMinimumDistancePenalty(double d);
1959  void setParamUseResponseExpansion(bool b);
1960  };
1961 } // namespace karto
1962 
1963 #endif // OPEN_KARTO_MAPPER_H
CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize, kt_double resolution, kt_double smearDeviation)
Definition: Mapper.h:909
std::map< Name, ScanManager * > ScanManagerMap
Definition: Mapper.h:1190
virtual ~ScanSolver()
Definition: Mapper.h:736
EdgeLabel * m_pLabel
Definition: Mapper.h:367
ScanMatcher * m_pSequentialScanMatcher
Definition: Mapper.h:1687
std::vector< Vertex< T > * > GetAdjacentVertices() const
Definition: Mapper.h:250
T * m_pObject
Definition: Mapper.h:283
kt_int32s GetStateId() const
Definition: Karto.h:4829
int32_t kt_int32s
Definition: Types.h:51
GraphTraversal(Graph< T > *pGraph)
Definition: Mapper.h:407
Matrix3 m_Covariance
Definition: Mapper.h:193
virtual ~CorrelationGrid()
Definition: Mapper.h:799
Vertex< T > * m_pTarget
Definition: Mapper.h:366
EdgeLabel * GetLabel()
Definition: Mapper.h:350
Parameter< kt_double > * m_pLinkScanMaximumDistance
Definition: Mapper.h:1773
virtual void Info(const std::string &)
Definition: Mapper.h:40
kt_double GetHeading() const
Definition: Karto.h:2064
kt_double Round(kt_double value)
Definition: Math.h:87
kt_bool TryCloseLoop(LocalizedRangeScan *pScan, const Name &rSensorName)
Definition: Mapper.h:1613
T * GetObject() const
Definition: Mapper.h:241
Mapper * m_pMapper
Definition: Mapper.h:1171
virtual void AddNode(Vertex< LocalizedRangeScan > *)
Definition: Mapper.h:755
Parameter< kt_double > * m_pCoarseAngleResolution
Definition: Mapper.h:1872
Parameter< kt_double > * m_pLoopMatchMaximumVarianceCoarse
Definition: Mapper.h:1801
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1204
Parameter< kt_double > * m_pMinimumTravelHeading
Definition: Mapper.h:1743
virtual void EndLoopClosure(const std::string &)
Definition: Mapper.h:74
#define forEachAs(listtype, list, iter)
Definition: Macros.h:69
void AddEdge(Edge< T > *pEdge)
Definition: Mapper.h:481
Parameter< kt_double > * m_pMinimumAnglePenalty
Definition: Mapper.h:1876
MapperSensorManager * GetMapperSensorManager() const
Definition: Mapper.h:1603
std::vector< MapperListener * > m_Listeners
Definition: Mapper.h:1694
virtual void Clear()
Definition: Mapper.h:783
virtual void RemoveNode(kt_int32s)
Definition: Mapper.h:762
Rectangle2< kt_int32s > m_Roi
Definition: Mapper.h:1012
const T & GetY() const
Definition: Karto.h:975
uint8_t kt_int8u
Definition: Types.h:46
virtual void RemoveConstraint(kt_int32s, kt_int32s)
Definition: Mapper.h:776
virtual ~GraphTraversal()
Definition: Mapper.h:415
std::vector< Name > GetSensorNames()
Definition: Mapper.h:1231
virtual void LoopClosureCheck(const std::string &)
Definition: Mapper.h:64
Parameter< kt_double > * m_pLoopSearchSpaceSmearDeviation
Definition: Mapper.h:1856
void Update(const Pose2 &rPose1, const Pose2 &rPose2, const Matrix3 &rCovariance)
Definition: Mapper.h:137
const Matrix3 & GetCovariance()
Definition: Mapper.h:184
Parameter< kt_int32u > * m_pLoopMatchMinimumChainSize
Definition: Mapper.h:1794
Parameter< kt_double > * m_pCorrelationSearchSpaceResolution
Definition: Mapper.h:1828
void Clear()
Definition: Mapper.h:489
Parameter< kt_double > * m_pMinimumDistancePenalty
Definition: Mapper.h:1877
void AddVertex(const Name &rName, Vertex< T > *pVertex)
Definition: Mapper.h:472
virtual void AddConstraint(Edge< LocalizedRangeScan > *)
Definition: Mapper.h:769
virtual ~Edge()
Definition: Mapper.h:315
CorrelationGrid * GetCorrelationGrid() const
Definition: Mapper.h:1121
Parameter< kt_double > * m_pDistanceVariancePenalty
Definition: Mapper.h:1864
Matrix3 Transpose() const
Definition: Karto.h:2430
Parameter< kt_bool > * m_pUseScanMatching
Definition: Mapper.h:1705
#define forEach(listtype, list)
Definition: Macros.h:66
kt_double m_SmearDeviation
Definition: Mapper.h:1003
Graph< T > * m_pGraph
Definition: Mapper.h:431
void SetROI(const Rectangle2< kt_int32s > &roi)
Definition: Mapper.h:855
const Rectangle2< kt_int32s > & GetROI() const
Definition: Mapper.h:846
MapperSensorManager * m_pMapperSensorManager
Definition: Mapper.h:1689
kt_int32u m_RunningBufferMaximumSize
Definition: Mapper.h:1333
GraphTraversal< LocalizedRangeScan > * m_pTraversal
Definition: Mapper.h:708
Parameter< kt_bool > * m_pDoLoopClosing
Definition: Mapper.h:1779
Parameter< kt_double > * m_pFineSearchAngleOffset
Definition: Mapper.h:1868
const T & GetX() const
Definition: Karto.h:957
Parameter< kt_double > * m_pScanBufferMaximumScanDistance
Definition: Mapper.h:1760
ScanSolver * m_pScanOptimizer
Definition: Mapper.h:1692
const Pose2 & GetPose2()
Definition: Mapper.h:166
virtual void Debug(const std::string &)
Definition: Mapper.h:52
Parameter< kt_double > * m_pLoopSearchSpaceDimension
Definition: Mapper.h:1844
void SmearPoint(const Vector2< kt_int32s > &rGridPoint)
Definition: Mapper.h:864
Pose2 m_Pose2
Definition: Mapper.h:191
const Name & GetSensorName() const
Definition: Karto.h:4883
ScanMatcher * GetLoopScanMatcher() const
Definition: Mapper.h:607
Parameter< kt_double > * m_pMinimumTravelDistance
Definition: Mapper.h:1733
std::vector< Edge< T > * > m_Edges
Definition: Mapper.h:535
Mapper * m_pMapper
Definition: Mapper.h:698
kt_int8u * m_pKernel
Definition: Mapper.h:1009
void AddEdge(Edge< T > *pEdge)
Definition: Mapper.h:278
void SetUseScanMatching(kt_bool val)
Definition: Mapper.h:1682
std::vector< Pose2 > Pose2Vector
Definition: Karto.h:2174
const VertexMap & GetVertices() const
Definition: Mapper.h:521
virtual ~Vertex()
Definition: Mapper.h:224
virtual kt_int32s GridIndex(const Vector2< kt_int32s > &rGrid, kt_bool boundaryCheck=true) const
Definition: Karto.h:4469
virtual ~MapperSensorManager()
Definition: Mapper.h:1206
ScanMatcher(Mapper *pMapper)
Definition: Mapper.h:1162
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:172
kt_double m_RunningBufferMaximumDistance
Definition: Mapper.h:1334
const std::vector< Edge< T > * > & GetEdges() const
Definition: Mapper.h:232
Pose2 m_PoseDifference
Definition: Mapper.h:192
#define const_forEach(listtype, list)
Definition: Macros.h:72
VertexMap m_Vertices
Definition: Mapper.h:530
Vertex< T > * m_pSource
Definition: Mapper.h:365
virtual void BeginLoopClosure(const std::string &)
Definition: Mapper.h:69
Pose2 TransformPose(const Pose2 &rSourcePose)
Definition: Karto.h:2890
GridIndexLookup< kt_int8u > * m_pGridLookup
Definition: Mapper.h:1176
bool kt_bool
Definition: Types.h:64
virtual ~Graph()
Definition: Mapper.h:461
static CorrelationGrid * CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution, kt_double smearDeviation)
Definition: Mapper.h:813
CorrelationGrid * m_pCorrelationGrid
Definition: Mapper.h:1173
Parameter< kt_double > * m_pLoopSearchSpaceResolution
Definition: Mapper.h:1850
kt_bool m_Initialized
Definition: Mapper.h:1685
Parameter< kt_double > * m_pCorrelationSearchSpaceDimension
Definition: Mapper.h:1822
MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
Definition: Mapper.h:1196
LocalizedRangeScan * GetScan(kt_int32s id)
Definition: Mapper.h:1260
kt_int32s m_KernelSize
Definition: Mapper.h:1006
std::vector< std::pair< kt_int32s, Pose2 > > IdPoseVector
Definition: Mapper.h:724
Parameter< kt_double > * m_pLinkMatchMinimumResponseFine
Definition: Mapper.h:1766
Vertex< LocalizedRangeScan > * GetVertex(LocalizedRangeScan *pScan)
Definition: Mapper.h:618
const Pose2 & GetPose1()
Definition: Mapper.h:157
virtual ~EdgeLabel()
Definition: Mapper.h:97
Pose2 m_Pose1
Definition: Mapper.h:190
ScanMatcher * m_pLoopScanMatcher
Definition: Mapper.h:703
Vertex(T *pObject)
Definition: Mapper.h:216
void SetLabel(EdgeLabel *pLabel)
Definition: Mapper.h:359
ScanManagerMap m_ScanManagers
Definition: Mapper.h:1331
Grid< kt_double > * m_pSearchSpaceProbs
Definition: Mapper.h:1174
std::map< Name, std::vector< Vertex< T > * > > VertexMap
Definition: Mapper.h:448
static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution)
Definition: Mapper.h:991
Edge(Vertex< T > *pSource, Vertex< T > *pTarget)
Definition: Mapper.h:303
virtual ~LinkInfo()
Definition: Mapper.h:126
double kt_double
Definition: Types.h:67
Parameter< kt_double > * m_pCorrelationSearchSpaceSmearDeviation
Definition: Mapper.h:1834
Definition: Karto.h:73
Vertex< T > * GetSource() const
Definition: Mapper.h:332
const std::vector< Edge< T > * > & GetEdges() const
Definition: Mapper.h:512
Vertex< T > * GetTarget() const
Definition: Mapper.h:341
virtual kt_int32s GridIndex(const Vector2< kt_int32s > &rGrid, kt_bool boundaryCheck=true) const
Definition: Mapper.h:834
MapperGraph * m_pGraph
Definition: Mapper.h:1691
Parameter< kt_double > * m_pLoopMatchMinimumResponseCoarse
Definition: Mapper.h:1807
std::vector< LocalizedRangeScan * > m_Scans
Definition: Mapper.h:1338
LinkInfo(const Pose2 &rPose1, const Pose2 &rPose2, const Matrix3 &rCovariance)
Definition: Mapper.h:118
Parameter< kt_double > * m_pCoarseSearchAngleOffset
Definition: Mapper.h:1869
Parameter< kt_double > * m_pLoopSearchMaximumDistance
Definition: Mapper.h:1786
Parameter< kt_double > * m_pAngleVariancePenalty
Definition: Mapper.h:1865
uint32_t kt_int32u
Definition: Types.h:52
virtual void CalculateKernel()
Definition: Mapper.h:927
Parameter< kt_double > * m_pMinimumTimeInterval
Definition: Mapper.h:1723
Parameter< kt_bool > * m_pUseResponseExpansion
Definition: Mapper.h:1880
Parameter< kt_int32u > * m_pScanBufferSize
Definition: Mapper.h:1753
#define KARTO_EXPORT
Definition: Macros.h:56
kt_bool IsUpTo(const T &value, const T &maximum)
Definition: Math.h:147
std::vector< Edge< T > * > m_Edges
Definition: Mapper.h:284
ScanManager * GetScanManager(LocalizedRangeScan *pScan)
Definition: Mapper.h:1309
std::vector< LocalizedRangeScan * > LocalizedRangeScanVector
Definition: Karto.h:5462
ScanManager * GetScanManager(const Name &rSensorName)
Definition: Mapper.h:1319
Parameter< kt_bool > * m_pUseScanBarycenter
Definition: Mapper.h:1710
const Pose2 & GetPoseDifference()
Definition: Mapper.h:175
Parameter< kt_double > * m_pLoopMatchMinimumResponseFine
Definition: Mapper.h:1813
void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
Definition: Karto.h:2395


open_karto
Author(s):
autogenerated on Sat Apr 6 2019 02:22:46