00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #pragma once
00019
00020 #ifndef __OpenKarto_Mapper_h__
00021 #define __OpenKarto_Mapper_h__
00022
00023 #ifdef USE_TBB
00024 #include <tbb/mutex.h>
00025 #include <tbb/parallel_for.h>
00026 #include <tbb/blocked_range.h>
00027 #include <tbb/blocked_range3d.h>
00028 #endif
00029
00030 #include <OpenKarto/Event.h>
00031 #include <OpenKarto/Pair.h>
00032 #include <OpenKarto/Geometry.h>
00033 #include <OpenKarto/StringHelper.h>
00034 #include <OpenKarto/SensorData.h>
00035 #include <OpenKarto/Grid.h>
00036 #include <OpenKarto/GridIndexLookup.h>
00037 #include <OpenKarto/Module.h>
00038 #include <OpenKarto/OccupancyGrid.h>
00039 #include <OpenKarto/TypeCasts.h>
00040
00041 namespace karto
00042 {
00043
00045
00046
00050 class MapperEventArguments : public EventArguments
00051 {
00052 public:
00057 MapperEventArguments(const String& rMessage)
00058 : m_Message(rMessage)
00059 {
00060 }
00061
00065 virtual ~MapperEventArguments()
00066 {
00067 }
00068
00069 public:
00074 const String& GetEventMessage() const
00075 {
00076 return m_Message;
00077 }
00078
00079 private:
00080 String m_Message;
00081 };
00082
00083 #ifdef WIN32
00084 EXPORT_KARTO_EVENT(KARTO_EXPORT, MapperEventArguments)
00085 #endif
00086
00090
00094 class EdgeLabel
00095 {
00096 public:
00100 EdgeLabel()
00101 {
00102 }
00103
00107 virtual ~EdgeLabel()
00108 {
00109 }
00110 };
00111
00115
00116
00117
00118
00119 class LinkInfo : public EdgeLabel
00120 {
00121 public:
00128 LinkInfo(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
00129 {
00130 Update(rPose1, rPose2, rCovariance);
00131 }
00132
00136 virtual ~LinkInfo()
00137 {
00138 }
00139
00140 public:
00147 void Update(const Pose2& rPose1, const Pose2& rPose2, const Matrix3& rCovariance)
00148 {
00149 m_Pose1 = rPose1;
00150 m_Pose2 = rPose2;
00151
00152
00153 Transform transform(rPose1, Pose2());
00154 m_PoseDifference = transform.TransformPose(rPose2);
00155
00156
00157 Matrix3 rotationMatrix;
00158 rotationMatrix.FromAxisAngle(0, 0, 1, -rPose1.GetHeading());
00159
00160 m_Covariance = rotationMatrix * rCovariance * rotationMatrix.Transpose();
00161 }
00162
00167 inline const Pose2& GetPose1()
00168 {
00169 return m_Pose1;
00170 }
00171
00176 inline const Pose2& GetPose2()
00177 {
00178 return m_Pose2;
00179 }
00180
00185 inline const Pose2& GetPoseDifference()
00186 {
00187 return m_PoseDifference;
00188 }
00189
00194 inline const Matrix3& GetCovariance()
00195 {
00196 return m_Covariance;
00197 }
00198
00199 private:
00200 Pose2 m_Pose1;
00201 Pose2 m_Pose2;
00202 Pose2 m_PoseDifference;
00203 Matrix3 m_Covariance;
00204 };
00205
00209
00210 template<typename T>
00211 class Edge;
00212
00216 template<typename T>
00217 class Vertex
00218 {
00219 friend class Edge<T>;
00220
00221 public:
00226 Vertex(T pObject)
00227 : m_pObject(pObject)
00228 {
00229 }
00230
00234 virtual ~Vertex()
00235 {
00236 }
00237
00242 inline const List<Edge<T>*>& GetEdges() const
00243 {
00244 return m_Edges;
00245 }
00246
00251 inline T GetVertexObject() const
00252 {
00253 return m_pObject;
00254 }
00255
00260 List<Vertex<T>*> GetAdjacentVertices() const
00261 {
00262 List<Vertex<T>*> vertices;
00263
00264 karto_const_forEach(typename List<Edge<T>*>, &m_Edges)
00265 {
00266 Edge<T>* pEdge = *iter;
00267
00268
00269 if (pEdge->GetSource() != this)
00270 {
00271 vertices.Add(pEdge->GetSource());
00272 }
00273
00274 if (pEdge->GetTarget() != this)
00275 {
00276 vertices.Add(pEdge->GetTarget());
00277 }
00278 }
00279
00280 return vertices;
00281 }
00282
00283 private:
00288 inline void AddEdge(Edge<T>* pEdge)
00289 {
00290 m_Edges.Add(pEdge);
00291 }
00292
00293 T m_pObject;
00294 List<Edge<T>*> m_Edges;
00295 };
00296
00300
00304 template<typename T>
00305 class Edge
00306 {
00307 public:
00313 Edge(Vertex<T>* pSource, Vertex<T>* pTarget)
00314 : m_pSource(pSource)
00315 , m_pTarget(pTarget)
00316 , m_pLabel(NULL)
00317 {
00318 m_pSource->AddEdge(this);
00319 m_pTarget->AddEdge(this);
00320 }
00321
00325 virtual ~Edge()
00326 {
00327 m_pSource = NULL;
00328 m_pTarget = NULL;
00329
00330 if (m_pLabel != NULL)
00331 {
00332 delete m_pLabel;
00333 m_pLabel = NULL;
00334 }
00335 }
00336
00337 public:
00342 inline Vertex<T>* GetSource() const
00343 {
00344 return m_pSource;
00345 }
00346
00351 inline Vertex<T>* GetTarget() const
00352 {
00353 return m_pTarget;
00354 }
00355
00360 inline EdgeLabel* GetLabel()
00361 {
00362 return m_pLabel;
00363 }
00364
00369 inline void SetLabel(EdgeLabel* pLabel)
00370 {
00371 m_pLabel = pLabel;
00372 }
00373
00374 private:
00375 Vertex<T>* m_pSource;
00376 Vertex<T>* m_pTarget;
00377 EdgeLabel* m_pLabel;
00378 };
00379
00383
00387 template<typename T>
00388 class Visitor
00389 {
00390 public:
00396 virtual kt_bool Visit(Vertex<T>* pVertex) = 0;
00397 };
00398
00402
00403 template<typename T>
00404 class Graph;
00405
00409 template<typename T>
00410 class GraphTraversal
00411 {
00412 public:
00417 GraphTraversal(Graph<T>* pGraph)
00418 : m_pGraph(pGraph)
00419 {
00420 }
00421
00425 virtual ~GraphTraversal()
00426 {
00427 }
00428
00429 public:
00436 virtual List<T> Traverse(Vertex<T>* pStartVertex, Visitor<T>* pVisitor) = 0;
00437
00438 protected:
00442 Graph<T>* m_pGraph;
00443 };
00444
00448
00452 template<typename T>
00453 class Graph
00454 {
00455 public:
00459 typedef List<Vertex<T>*> VertexList;
00460
00464 typedef List<Edge<T>*> EdgeList;
00465
00466 public:
00470 Graph()
00471 {
00472 }
00473
00477 virtual ~Graph()
00478 {
00479 Clear();
00480 }
00481
00482 public:
00487 inline void AddVertex(Vertex<T>* pVertex)
00488 {
00489 m_Vertices.Add(pVertex);
00490 }
00491
00496 inline void AddEdge(Edge<T>* pEdge)
00497 {
00498 m_Edges.Add(pEdge);
00499 }
00500
00504 void Clear()
00505 {
00506 karto_const_forEach(typename VertexList, &m_Vertices)
00507 {
00508
00509 delete *iter;
00510 }
00511
00512 m_Vertices.Clear();
00513
00514 karto_const_forEach(typename EdgeList, &m_Edges)
00515 {
00516
00517 delete *iter;
00518 }
00519
00520 m_Edges.Clear();
00521 }
00522
00527 inline const EdgeList& GetEdges() const
00528 {
00529 return m_Edges;
00530 }
00531
00536 inline const VertexList& GetVertices() const
00537 {
00538 return m_Vertices;
00539 }
00540
00541 protected:
00545 VertexList m_Vertices;
00546
00550 EdgeList m_Edges;
00551 };
00552
00556
00557 class OpenMapper;
00558 class ScanMatcher;
00559
00563 class KARTO_EXPORT MapperGraph : public Graph<LocalizedObjectPtr>
00564 {
00565 public:
00571 MapperGraph(OpenMapper* pOpenMapper, kt_double rangeThreshold);
00572
00576 virtual ~MapperGraph();
00577
00578 public:
00583 void AddVertex(LocalizedObject* pObject);
00584
00593 Edge<LocalizedObjectPtr>* AddEdge(LocalizedObject* pSourceObject, LocalizedObject* pTargetObject, kt_bool& rIsNewEdge);
00594
00599 void AddEdges(LocalizedObject* pObject);
00600
00606 void AddEdges(LocalizedLaserScan* pScan, const Matrix3& rCovariance);
00607
00614 kt_bool TryCloseLoop(LocalizedLaserScan* pScan, const Identifier& rSensorName);
00615
00622 LocalizedLaserScanList FindNearLinkedScans(LocalizedLaserScan* pScan, kt_double maxDistance);
00623
00629 LocalizedLaserScanList FindOverlappingScans(LocalizedLaserScan* pScan);
00630
00635 inline ScanMatcher* GetLoopScanMatcher() const
00636 {
00637 return m_pLoopScanMatcher;
00638 }
00639
00647 void LinkChainToScan(const LocalizedLaserScanList& rChain, LocalizedLaserScan* pScan, const Pose2& rMean, const Matrix3& rCovariance);
00648
00649 private:
00655 inline Vertex<LocalizedObjectPtr>* GetVertex(LocalizedObject* pObject)
00656 {
00657 return m_Vertices[pObject->GetUniqueId()];
00658 }
00659
00665 LocalizedLaserScan* GetClosestScanToPose(const LocalizedLaserScanList& rScans, const Pose2& rPose) const;
00666
00674 void LinkObjects(LocalizedObject* pFromObject, LocalizedObject* pToObject, const Pose2& rMean, const Matrix3& rCovariance);
00675
00682 void LinkNearChains(LocalizedLaserScan* pScan, Pose2List& rMeans, List<Matrix3>& rCovariances);
00683
00689 List<LocalizedLaserScanList> FindNearChains(LocalizedLaserScan* pScan);
00690
00697 Pose2 ComputeWeightedMean(const Pose2List& rMeans, const List<Matrix3>& rCovariances) const;
00698
00707 LocalizedLaserScanList FindPossibleLoopClosure(LocalizedLaserScan* pScan, const Identifier& rSensorName, kt_int32u& rStartScanIndex);
00708
00712 void CorrectPoses();
00713
00714 private:
00718 OpenMapper* m_pOpenMapper;
00719
00723 ScanMatcher* m_pLoopScanMatcher;
00724
00728 GraphTraversal<LocalizedObjectPtr>* m_pTraversal;
00729 };
00730
00734
00738 class ScanSolver : public Referenced
00739 {
00740 public:
00744 typedef List<Pair<kt_int32s, Pose2> > IdPoseVector;
00745
00749 ScanSolver()
00750 {
00751 }
00752
00753 protected:
00754
00758 virtual ~ScanSolver()
00759 {
00760 }
00761
00762
00763 public:
00767 virtual void Compute() = 0;
00768
00773 virtual const IdPoseVector& GetCorrections() const = 0;
00774
00778 virtual void AddNode(Vertex<LocalizedObjectPtr>* )
00779 {
00780 }
00781
00785 virtual void RemoveNode(kt_int32s )
00786 {
00787 }
00788
00792 virtual void AddConstraint(Edge<LocalizedObjectPtr>* )
00793 {
00794 }
00795
00799 virtual void RemoveConstraint(kt_int32s , kt_int32s )
00800 {
00801 }
00802
00806 virtual void Clear() {};
00807 };
00808
00812
00816 class CorrelationGrid : public Grid<kt_int8u>
00817 {
00818 protected:
00819
00823 virtual ~CorrelationGrid()
00824 {
00825 delete [] m_pKernel;
00826 }
00827
00828
00829 public:
00838 static CorrelationGrid* CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution, kt_double smearDeviation)
00839 {
00840 assert(resolution != 0.0);
00841
00842
00843 kt_int32u borderSize = GetHalfKernelSize(smearDeviation, resolution) + 1;
00844
00845 CorrelationGrid* pGrid = new CorrelationGrid(width, height, borderSize, resolution, smearDeviation);
00846
00847 return pGrid;
00848 }
00849
00856 virtual kt_int32s GridIndex(const Vector2i& rGrid, kt_bool boundaryCheck = true) const
00857 {
00858 kt_int32s x = rGrid.GetX() + m_Roi.GetX();
00859 kt_int32s y = rGrid.GetY() + m_Roi.GetY();
00860
00861 return Grid<kt_int8u>::GridIndex(Vector2i(x, y), boundaryCheck);
00862 }
00863
00868 inline const Rectangle2<kt_int32s>& GetROI() const
00869 {
00870 return m_Roi;
00871 }
00872
00877 inline void SetROI(const Rectangle2<kt_int32s>& roi)
00878 {
00879 m_Roi = roi;
00880 }
00881
00886 inline void SmearPoint(const Vector2i& rGridPoint)
00887 {
00888 assert(m_pKernel != NULL);
00889
00890 int gridIndex = GridIndex(rGridPoint);
00891 if (GetDataPointer()[gridIndex] != GridStates_Occupied)
00892 {
00893 return;
00894 }
00895
00896 kt_int32s halfKernel = m_KernelSize / 2;
00897
00898
00899 for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
00900 {
00901 kt_int8u* pGridAdr = GetDataPointer(Vector2i(rGridPoint.GetX(), rGridPoint.GetY() + j));
00902
00903 kt_int32s kernelConstant = (halfKernel) + m_KernelSize * (j + halfKernel);
00904
00905
00906
00907
00908 SmearInternal(halfKernel, kernelConstant, pGridAdr);
00909 }
00910 }
00911
00912 protected:
00921 CorrelationGrid(kt_int32u width, kt_int32u height, kt_int32u borderSize, kt_double resolution, kt_double smearDeviation)
00922 : Grid<kt_int8u>(width + borderSize * 2, height + borderSize * 2)
00923 , m_SmearDeviation(smearDeviation)
00924 , m_pKernel(NULL)
00925 {
00926 GetCoordinateConverter()->SetScale(1.0 / resolution);
00927
00928
00929 m_Roi = Rectangle2<kt_int32s>(borderSize, borderSize, width, height);
00930
00931
00932 CalculateKernel();
00933 }
00934
00938 virtual void CalculateKernel()
00939 {
00940 kt_double resolution = GetResolution();
00941
00942 assert(resolution != 0.0);
00943 assert(m_SmearDeviation != 0.0);
00944
00945
00946
00947 const kt_double MIN_SMEAR_DISTANCE_DEVIATION = 0.5 * resolution;
00948 const kt_double MAX_SMEAR_DISTANCE_DEVIATION = 10 * resolution;
00949
00950
00951 if (!math::InRange(m_SmearDeviation, MIN_SMEAR_DISTANCE_DEVIATION, MAX_SMEAR_DISTANCE_DEVIATION))
00952 {
00953 StringBuilder error;
00954 error << "Mapper Error: Smear deviation too small: Must be between " << MIN_SMEAR_DISTANCE_DEVIATION << " and " << MAX_SMEAR_DISTANCE_DEVIATION;
00955 throw Exception(error.ToString());
00956 }
00957
00958
00959
00960
00961 m_KernelSize = 2 * GetHalfKernelSize(m_SmearDeviation, resolution) + 1;
00962
00963
00964 m_pKernel = new kt_int8u[m_KernelSize * m_KernelSize];
00965 if (m_pKernel == NULL)
00966 {
00967 throw Exception("Unable to allocate memory for kernel!");
00968 }
00969
00970
00971 kt_int32s halfKernel = m_KernelSize / 2;
00972 for (kt_int32s i = -halfKernel; i <= halfKernel; i++)
00973 {
00974 for (kt_int32s j = -halfKernel; j <= halfKernel; j++)
00975 {
00976 #ifdef WIN32
00977 kt_double distanceFromMean = _hypot(i * resolution, j * resolution);
00978 #else
00979 kt_double distanceFromMean = hypot(i * resolution, j * resolution);
00980 #endif
00981 kt_double z = exp(-0.5 * pow(distanceFromMean / m_SmearDeviation, 2));
00982
00983 kt_int32u kernelValue = static_cast<kt_int32u>(math::Round(z * GridStates_Occupied));
00984 assert(math::IsUpTo(kernelValue, static_cast<kt_int32u>(255)));
00985
00986 int kernelArrayIndex = (i + halfKernel) + m_KernelSize * (j + halfKernel);
00987 m_pKernel[kernelArrayIndex] = static_cast<kt_int8u>(kernelValue);
00988 }
00989 }
00990 }
00991
00999 static kt_int32s GetHalfKernelSize(kt_double smearDeviation, kt_double resolution)
01000 {
01001 assert(resolution != 0.0);
01002
01003 return static_cast<kt_int32s>(math::Round(2.0 * smearDeviation / resolution));
01004 }
01005
01006 private:
01007
01008 inline void SmearInternal(kt_int32s halfKernel, kt_int32s kernelConstant, kt_int8u* pGridAdr)
01009 {
01010 kt_int8u kernelValue;
01011 kt_int32s kernelArrayIndex;
01012 kt_int32s i;
01013
01014 for (i = -halfKernel; i <= halfKernel; i++)
01015 {
01016 kernelArrayIndex = i + kernelConstant;
01017
01018 kernelValue = m_pKernel[kernelArrayIndex];
01019 if (kernelValue > pGridAdr[i])
01020 {
01021
01022 pGridAdr[i] = kernelValue;
01023 }
01024 }
01025 }
01026
01031 kt_double m_SmearDeviation;
01032
01033
01034 kt_int32s m_KernelSize;
01035
01036
01037 kt_int8u* m_pKernel;
01038
01039
01040 Rectangle2<kt_int32s> m_Roi;
01041 };
01042
01046
01047 class ScanMatcherGridSet : public Referenced
01048 {
01049 public:
01050 ScanMatcherGridSet(CorrelationGrid* pCorrelationGrid,
01051 Grid<kt_double>* pSearchSpaceProbs,
01052 GridIndexLookup<kt_int8u>* pGridLookup)
01053 : m_pCorrelationGrid(pCorrelationGrid)
01054 , m_pSearchSpaceProbs(pSearchSpaceProbs)
01055 , m_pGridLookup(pGridLookup)
01056 {
01057 }
01058
01059 virtual ~ScanMatcherGridSet()
01060 {
01061 delete m_pGridLookup;
01062 }
01063
01064 SmartPointer<CorrelationGrid> m_pCorrelationGrid;
01065 SmartPointer<Grid<kt_double> > m_pSearchSpaceProbs;
01066 GridIndexLookup<kt_int8u>* m_pGridLookup;
01067 };
01068
01072
01073 class ScanMatcherGridSetBank;
01074
01078 class KARTO_EXPORT ScanMatcher
01079 {
01080 public:
01084 virtual ~ScanMatcher();
01085
01086 public:
01096 static ScanMatcher* Create(OpenMapper* pOpenMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold);
01097
01108 kt_double MatchScan(LocalizedLaserScan* pScan, const LocalizedLaserScanList& rBaseScans, Pose2& rMean, Matrix3& rCovariance,
01109 kt_bool doPenalize = true, kt_bool doRefineMatch = true);
01110
01128 kt_double CorrelateScan(ScanMatcherGridSet* pScanMatcherGridSet, LocalizedLaserScan* pScan, const Pose2& rSearchCenter, const Vector2d& rSearchSpaceOffset, const Vector2d& rSearchSpaceResolution,
01129 kt_double searchAngleOffset, kt_double searchAngleResolution, kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch);
01130
01142 static void ComputePositionalCovariance(Grid<kt_double>* pSearchSpaceProbs, const Pose2& rBestPose, kt_double bestResponse, const Pose2& rSearchCenter,
01143 const Vector2d& rSearchSpaceOffset, const Vector2d& rSearchSpaceResolution,
01144 kt_double searchAngleResolution, Matrix3& rCovariance);
01145
01156 static void ComputeAngularCovariance(ScanMatcherGridSet* pScanMatcherGridSet, const Pose2& rBestPose, kt_double bestResponse, const Pose2& rSearchCenter,
01157 kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3& rCovariance);
01158
01165 CorrelationGrid* GetCorrelationGrid() const;
01166
01173 Grid<kt_double>* GetSearchGrid() const;
01174
01182 static kt_double GetResponse(ScanMatcherGridSet* pScanMatcherGridSet, kt_int32u angleIndex, kt_int32s gridPositionIndex);
01183
01184 private:
01191 static void AddScans(CorrelationGrid* pCorrelationGrid, const LocalizedLaserScanList& rScans, const Vector2d& rViewPoint);
01192 static void AddScansNew(CorrelationGrid* pCorrelationGrid, const LocalizedLaserScanList& rScans, const Vector2d& rViewPoint);
01193
01201 static void AddScan(CorrelationGrid* pCorrelationGrid, LocalizedLaserScan* pScan, const Vector2d& rViewPoint, kt_bool doSmear = true);
01202 static void AddScanNew(CorrelationGrid* pCorrelationGrid, const Vector2dList& rValidPoints, kt_bool doSmear = true);
01203
01210 static Vector2dList FindValidPoints(LocalizedLaserScan* pScan, const Vector2d& rViewPoint);
01211
01212 protected:
01216 ScanMatcher(OpenMapper* pOpenMapper)
01217 : m_pOpenMapper(pOpenMapper)
01218 , m_pScanMatcherGridSet(NULL)
01219 , m_pScanMatcherGridSetBank(NULL)
01220 {
01221 }
01222
01223 private:
01224 OpenMapper* m_pOpenMapper;
01225
01226 SmartPointer<ScanMatcherGridSet> m_pScanMatcherGridSet;
01227 ScanMatcherGridSetBank* m_pScanMatcherGridSetBank;
01228 };
01229
01233
01234 class SensorDataManager;
01235 struct MapperSensorManagerPrivate;
01236
01240 class KARTO_EXPORT MapperSensorManager
01241 {
01242 public:
01248 MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance);
01249
01253 virtual ~MapperSensorManager();
01254
01255 public:
01261 void RegisterSensor(const Identifier& rSensorName);
01262
01269 LocalizedObject* GetLocalizedObject(const Identifier& rSensorName, kt_int32s stateId);
01270
01275 List<Identifier> GetSensorNames();
01276
01282 LocalizedLaserScan* GetLastScan(const Identifier& rSensorName);
01283
01288 void SetLastScan(LocalizedLaserScan* pScan);
01289
01294 void ClearLastScan(const Identifier& rSensorName);
01295
01301 LocalizedObject* GetLocalizedObject(kt_int32s uniqueId);
01302
01307 void AddLocalizedObject(LocalizedObject* pObject);
01308
01313 void AddRunningScan(LocalizedLaserScan* pScan);
01314
01320 LocalizedLaserScanList& GetScans(const Identifier& rSensorName);
01321
01328 kt_int32s GetScanIndex(LocalizedLaserScan* pScan);
01329
01335 LocalizedLaserScanList& GetRunningScans(const Identifier& rSensorName);
01336
01341 LocalizedLaserScanList GetAllScans();
01342
01347 LocalizedObjectList GetAllObjects();
01348
01352 void Clear();
01353
01354 private:
01359 inline SensorDataManager* GetSensorDataManager(LocalizedObject* pObject)
01360 {
01361 return GetSensorDataManager(pObject->GetSensorIdentifier());
01362 }
01363
01369 SensorDataManager* GetSensorDataManager(const Identifier& rSensorName);
01370
01371 private:
01372 MapperSensorManagerPrivate* m_pMapperSensorManagerPrivate;
01373 };
01374
01378
01531 class KARTO_EXPORT OpenMapper : public Module
01532 {
01533 friend class MapperGraph;
01534 friend class ScanMatcher;
01535
01536 public:
01541 OpenMapper(kt_bool multiThreaded = true);
01542
01548 OpenMapper(const char* pName, kt_bool multiThreaded = true);
01549
01550 public:
01556 BasicEvent<MapperEventArguments> Message;
01557
01563 BasicEvent<MapperEventArguments> PreLoopClosed;
01564
01568 BasicEvent<MapperEventArguments> PostLoopClosed;
01569
01574 BasicEvent<EventArguments> ScansUpdated;
01575
01576 protected:
01577
01581 virtual ~OpenMapper();
01582
01583
01584 public:
01589 inline kt_bool IsMultiThreaded()
01590 {
01591 return m_MultiThreaded;
01592 }
01593
01599 void Initialize(kt_double rangeThreshold);
01600
01605 void Reset();
01606
01614 virtual kt_bool Process(Object* pObject);
01615
01622 const LocalizedLaserScanList GetAllProcessedScans() const;
01623
01630 const LocalizedObjectList GetAllProcessedObjects() const;
01631
01636 ScanSolver* GetScanSolver() const;
01637
01642 void SetScanSolver(ScanSolver* pSolver);
01643
01648 virtual MapperGraph* GetGraph() const;
01649
01654 ScanMatcher* GetSequentialScanMatcher() const;
01655
01660 ScanMatcher* GetLoopScanMatcher() const;
01661
01666 inline MapperSensorManager* GetMapperSensorManager() const
01667 {
01668 return m_pMapperSensorManager;
01669 }
01670
01677 inline kt_bool TryCloseLoop(LocalizedLaserScan* pScan, const Identifier& rSensorName)
01678 {
01679 return m_pGraph->TryCloseLoop(pScan, rSensorName);
01680 }
01681
01682 protected:
01687 virtual void ScanMatched(LocalizedLaserScan* pScan) {};
01688
01693 virtual void ScanMatchingEnd(LocalizedLaserScan* pScan) {};
01694
01695 private:
01696 void InitializeParameters();
01697
01705 kt_bool HasMovedEnough(LocalizedLaserScan* pScan, LocalizedLaserScan* pLastScan) const;
01706
01707 public:
01709
01710
01714 kt_bool IsInitialized()
01715 {
01716 return m_Initialized;
01717 }
01718
01719 private:
01720
01721 OpenMapper(const OpenMapper&);
01722 const OpenMapper& operator=(const OpenMapper&);
01723
01724 protected:
01728 SmartPointer<ScanSolver> m_pScanSolver;
01729
01730 private:
01731 kt_bool m_Initialized;
01732 kt_bool m_MultiThreaded;
01733
01734 ScanMatcher* m_pSequentialScanMatcher;
01735
01736 MapperSensorManager* m_pMapperSensorManager;
01737
01738 MapperGraph* m_pGraph;
01739
01741
01742
01744
01745 Parameter<kt_bool>* m_pUseScanMatching;
01746 Parameter<kt_bool>* m_pUseScanBarycenter;
01747 Parameter<kt_double>* m_pMinimumTravelDistance;
01748 Parameter<kt_double>* m_pMinimumTravelHeading;
01749
01751
01752
01753 Parameter<kt_int32u>* m_pScanBufferSize;
01754 Parameter<kt_double>* m_pScanBufferMaximumScanDistance;
01755 Parameter<kt_bool>* m_pUseResponseExpansion;
01756
01757 Parameter<kt_double>* m_pDistanceVariancePenalty;
01758 Parameter<kt_double>* m_pMinimumDistancePenalty;
01759 Parameter<kt_double>* m_pAngleVariancePenalty;
01760 Parameter<kt_double>* m_pMinimumAnglePenalty;
01761
01762 Parameter<kt_double>* m_pLinkMatchMinimumResponseFine;
01763 Parameter<kt_double>* m_pLinkScanMaximumDistance;
01764
01766
01767
01768 Parameter<kt_double>* m_pCorrelationSearchSpaceDimension;
01769 Parameter<kt_double>* m_pCorrelationSearchSpaceResolution;
01770 Parameter<kt_double>* m_pCorrelationSearchSpaceSmearDeviation;
01771 Parameter<kt_double>* m_pCoarseSearchAngleOffset;
01772 Parameter<kt_double>* m_pFineSearchAngleOffset;
01773 Parameter<kt_double>* m_pCoarseAngleResolution;
01774
01776
01777
01778 Parameter<kt_double>* m_pLoopSearchSpaceDimension;
01779 Parameter<kt_double>* m_pLoopSearchSpaceResolution;
01780 Parameter<kt_double>* m_pLoopSearchSpaceSmearDeviation;
01781
01783
01784
01785 Parameter<kt_double>* m_pLoopSearchMaximumDistance;
01786 Parameter<kt_int32u>* m_pLoopMatchMinimumChainSize;
01787 Parameter<kt_double>* m_pLoopMatchMaximumVarianceCoarse;
01788 Parameter<kt_double>* m_pLoopMatchMinimumResponseCoarse;
01789 Parameter<kt_double>* m_pLoopMatchMinimumResponseFine;
01790 };
01791
01793 }
01794
01795 #endif // __OpenKarto_Mapper_h__