00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifdef USE_TBB
00019 #include <tbb/tbb_thread.h>
00020 #include <tbb/concurrent_queue.h>
00021 #endif
00022
00023 #include <stdexcept>
00024 #include <queue>
00025 #include <set>
00026 #include <iterator>
00027 #include <map>
00028 #include <iostream>
00029
00030 #include <OpenKarto/OpenMapper.h>
00031 #include <OpenKarto/Logger.h>
00032
00033 namespace karto
00034 {
00035
00036
00037
00038
00039
00040 #define MAX_VARIANCE 500.0
00041 #define DISTANCE_PENALTY_GAIN 0.2
00042 #define ANGLE_PENALTY_GAIN 0.2
00043
00047
00051 class SensorDataManager
00052 {
00053 public:
00057 SensorDataManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
00058 : m_pLastScan(NULL)
00059 , m_RunningBufferMaximumSize(runningBufferMaximumSize)
00060 , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
00061 {
00062 }
00063
00067 virtual ~SensorDataManager()
00068 {
00069 Clear();
00070 }
00071
00072 public:
00079 inline void AddObject(LocalizedObject* pObject, kt_int32s uniqueId)
00080 {
00081
00082 pObject->SetStateId(static_cast<kt_int32u>(m_Objects.Size()));
00083
00084
00085 pObject->SetUniqueId(uniqueId);
00086
00087 m_Objects.Add(pObject);
00088
00089
00090 LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
00091 if (pScan != NULL)
00092 {
00093 m_Scans.Add(pScan);
00094 }
00095 }
00096
00101 inline LocalizedLaserScan* GetLastScan()
00102 {
00103 return m_pLastScan;
00104 }
00105
00110 inline void SetLastScan(LocalizedLaserScan* pScan)
00111 {
00112 m_pLastScan = pScan;
00113 }
00114
00119 inline LocalizedObjectList& GetObjects()
00120 {
00121 return m_Objects;
00122 }
00123
00128 inline LocalizedLaserScanList& GetScans()
00129 {
00130 return m_Scans;
00131 }
00132
00138 inline kt_int32s GetScanIndex(LocalizedLaserScan* pScan)
00139 {
00140 LocalizedLaserScanPtr pSmartScan(pScan);
00141 return m_Scans.BinarySearch(pSmartScan, ScanIndexComparator);
00142 }
00143
00148 inline LocalizedLaserScanList& GetRunningScans()
00149 {
00150 return m_RunningScans;
00151 }
00152
00157 void AddRunningScan(LocalizedLaserScan* pScan)
00158 {
00159 m_RunningScans.Add(pScan);
00160
00161
00162 Pose2 frontScanPose = m_RunningScans.Front()->GetSensorPose();
00163 Pose2 backScanPose = m_RunningScans.Back()->GetSensorPose();
00164
00165
00166 kt_double squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
00167 while (m_RunningScans.Size() > m_RunningBufferMaximumSize || squaredDistance > math::Square(m_RunningBufferMaximumDistance) - KT_TOLERANCE)
00168 {
00169
00170 m_RunningScans.Remove(m_RunningScans.Front());
00171
00172
00173 frontScanPose = m_RunningScans.Front()->GetSensorPose();
00174 backScanPose = m_RunningScans.Back()->GetSensorPose();
00175 squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
00176 }
00177 }
00178
00182 void Clear()
00183 {
00184 m_Objects.Clear();
00185 m_Scans.Clear();
00186 m_RunningScans.Clear();
00187 m_pLastScan = NULL;
00188 }
00189
00190 private:
00191 static kt_int32s ScanIndexComparator(const LocalizedLaserScanPtr& pScan1, const LocalizedLaserScanPtr& pScan2)
00192 {
00193 return pScan1->GetStateId() - pScan2->GetStateId();
00194 }
00195
00196 private:
00197 LocalizedObjectList m_Objects;
00198
00199 LocalizedLaserScanList m_Scans;
00200 LocalizedLaserScanList m_RunningScans;
00201 LocalizedLaserScanPtr m_pLastScan;
00202
00203 kt_int32u m_RunningBufferMaximumSize;
00204 kt_double m_RunningBufferMaximumDistance;
00205 };
00206
00210
00211 typedef std::map<Identifier, SensorDataManager*> SensorDataManagerMap;
00212
00213 struct MapperSensorManagerPrivate
00214 {
00215
00216 SensorDataManagerMap m_SensorDataManagers;
00217
00218 kt_int32u m_RunningBufferMaximumSize;
00219 kt_double m_RunningBufferMaximumDistance;
00220
00221 kt_int32s m_NextUniqueId;
00222
00223 LocalizedObjectList m_Objects;
00224 };
00225
00226 MapperSensorManager::MapperSensorManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
00227 : m_pMapperSensorManagerPrivate(new MapperSensorManagerPrivate())
00228 {
00229 m_pMapperSensorManagerPrivate->m_RunningBufferMaximumSize = runningBufferMaximumSize;
00230 m_pMapperSensorManagerPrivate->m_RunningBufferMaximumDistance = runningBufferMaximumDistance;
00231 m_pMapperSensorManagerPrivate->m_NextUniqueId = 0;
00232 }
00233
00234 MapperSensorManager::~MapperSensorManager()
00235 {
00236 Clear();
00237 delete m_pMapperSensorManagerPrivate;
00238 }
00239
00240
00241 void MapperSensorManager::RegisterSensor(const Identifier& rSensorName)
00242 {
00243 if (GetSensorDataManager(rSensorName) == NULL)
00244 {
00245 m_pMapperSensorManagerPrivate->m_SensorDataManagers[rSensorName] = new SensorDataManager(m_pMapperSensorManagerPrivate->m_RunningBufferMaximumSize, m_pMapperSensorManagerPrivate->m_RunningBufferMaximumDistance);
00246 }
00247 }
00248
00249 LocalizedObject* MapperSensorManager::GetLocalizedObject(const Identifier& rSensorName, kt_int32s stateId)
00250 {
00251 SensorDataManager* pSensorDataManager = GetSensorDataManager(rSensorName);
00252 if (pSensorDataManager != NULL)
00253 {
00254 return pSensorDataManager->GetObjects().Get(stateId);
00255 }
00256
00257 assert(false);
00258 return NULL;
00259 }
00260
00261
00262 LocalizedObject* MapperSensorManager::GetLocalizedObject(kt_int32s uniqueId)
00263 {
00264 assert(math::IsUpTo(uniqueId, (kt_int32s)m_pMapperSensorManagerPrivate->m_Objects.Size()));
00265 return m_pMapperSensorManagerPrivate->m_Objects[uniqueId];
00266 }
00267
00268 List<Identifier> MapperSensorManager::GetSensorNames()
00269 {
00270 List<Identifier> sensorNames;
00271 const_forEach(SensorDataManagerMap, &m_pMapperSensorManagerPrivate->m_SensorDataManagers)
00272 {
00273 sensorNames.Add(iter->first);
00274 }
00275
00276 return sensorNames;
00277 }
00278
00279 LocalizedLaserScan* MapperSensorManager::GetLastScan(const Identifier& rSensorName)
00280 {
00281 return GetSensorDataManager(rSensorName)->GetLastScan();
00282 }
00283
00284 void MapperSensorManager::SetLastScan(LocalizedLaserScan* pScan)
00285 {
00286 GetSensorDataManager(pScan)->SetLastScan(pScan);
00287 }
00288
00289 void MapperSensorManager::ClearLastScan(const Identifier& rSensorName)
00290 {
00291 GetSensorDataManager(rSensorName)->SetLastScan(NULL);
00292 }
00293
00294 void MapperSensorManager::AddLocalizedObject(LocalizedObject* pObject)
00295 {
00296 GetSensorDataManager(pObject)->AddObject(pObject, m_pMapperSensorManagerPrivate->m_NextUniqueId);
00297 m_pMapperSensorManagerPrivate->m_Objects.Add(pObject);
00298 m_pMapperSensorManagerPrivate->m_NextUniqueId++;
00299 }
00300
00301 void MapperSensorManager::AddRunningScan(LocalizedLaserScan* pScan)
00302 {
00303 GetSensorDataManager(pScan)->AddRunningScan(pScan);
00304 }
00305
00306 LocalizedLaserScanList& MapperSensorManager::GetScans(const Identifier& rSensorName)
00307 {
00308 return GetSensorDataManager(rSensorName)->GetScans();
00309 }
00310
00311 kt_int32s MapperSensorManager::GetScanIndex(LocalizedLaserScan* pScan)
00312 {
00313 return GetSensorDataManager(pScan->GetSensorIdentifier())->GetScanIndex(pScan);
00314 }
00315
00316 LocalizedLaserScanList& MapperSensorManager::GetRunningScans(const Identifier& rSensorName)
00317 {
00318 return GetSensorDataManager(rSensorName)->GetRunningScans();
00319 }
00320
00321 LocalizedLaserScanList MapperSensorManager::GetAllScans()
00322 {
00323 LocalizedLaserScanList scans;
00324
00325 forEach(SensorDataManagerMap, &m_pMapperSensorManagerPrivate->m_SensorDataManagers)
00326 {
00327 LocalizedLaserScanList& rScans = iter->second->GetScans();
00328 scans.Add(rScans);
00329 }
00330
00331 return scans;
00332 }
00333
00334 karto::LocalizedObjectList MapperSensorManager::GetAllObjects()
00335 {
00336 LocalizedObjectList objects;
00337
00338 forEach(SensorDataManagerMap, &m_pMapperSensorManagerPrivate->m_SensorDataManagers)
00339 {
00340 LocalizedObjectList& rObjects = iter->second->GetObjects();
00341 objects.Add(rObjects);
00342 }
00343
00344 return objects;
00345 }
00346
00347 void MapperSensorManager::Clear()
00348 {
00349 forEach(SensorDataManagerMap, &m_pMapperSensorManagerPrivate->m_SensorDataManagers)
00350 {
00351 delete iter->second;
00352 }
00353
00354 m_pMapperSensorManagerPrivate->m_SensorDataManagers.clear();
00355 }
00356
00357 SensorDataManager* MapperSensorManager::GetSensorDataManager(const Identifier& rSensorName)
00358 {
00359 if (m_pMapperSensorManagerPrivate->m_SensorDataManagers.find(rSensorName) != m_pMapperSensorManagerPrivate->m_SensorDataManagers.end())
00360 {
00361 return m_pMapperSensorManagerPrivate->m_SensorDataManagers[rSensorName];
00362 }
00363
00364 return NULL;
00365 }
00366
00370
00371 #ifdef USE_TBB
00372 class ScanMatcherGridSetBank
00373 {
00374 public:
00375 ScanMatcherGridSetBank(kt_int32u nGrids, kt_int32s corrGridWidth, kt_int32s corrGridHeight, kt_double resolution, kt_double smearDeviation,
00376 kt_int32s searchSpaceGridWidth, kt_int32s searchSpaceGridHeight)
00377 {
00378 if (nGrids <= 0)
00379 {
00380 throw Exception("ScanMatcherGridSetBank requires at least 1 grid: " + StringHelper::ToString(nGrids));
00381 assert(false);
00382 }
00383
00384 for (kt_int32u i = 0; i < nGrids; i++)
00385 {
00386 CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(corrGridWidth, corrGridHeight, resolution, smearDeviation);
00387 Grid<kt_double>* pSearchSpaceProbs = Grid<kt_double>::CreateGrid(searchSpaceGridWidth, searchSpaceGridHeight, resolution);
00388 GridIndexLookup<kt_int8u>* pGridLookup = new GridIndexLookup<kt_int8u>(pCorrelationGrid);
00389
00390 m_ScanMatcherGridSets.push(new ScanMatcherGridSet(pCorrelationGrid, pSearchSpaceProbs, pGridLookup));
00391 }
00392 }
00393
00394 virtual ~ScanMatcherGridSetBank()
00395 {
00396
00397 m_ScanMatcherGridSets.push(NULL);
00398
00399 m_ScanMatcherGridSets.clear();
00400 }
00401
00402 public:
00406 SmartPointer<ScanMatcherGridSet> CheckOut()
00407 {
00408 SmartPointer<ScanMatcherGridSet> pScanMatcherGridSet = NULL;
00409
00410 m_ScanMatcherGridSets.pop(pScanMatcherGridSet);
00411
00412 return pScanMatcherGridSet;
00413 }
00414
00418 void Return(SmartPointer<ScanMatcherGridSet> pScanMatcherGridSet)
00419 {
00420 m_ScanMatcherGridSets.push(pScanMatcherGridSet);
00421 }
00422
00423 private:
00424 tbb::concurrent_bounded_queue<SmartPointer<ScanMatcherGridSet> > m_ScanMatcherGridSets;
00425 };
00426 #else
00427 class ScanMatcherGridSetBank
00428 {
00429 };
00430 #endif
00431
00435
00436 ScanMatcher::~ScanMatcher()
00437 {
00438 delete m_pScanMatcherGridSetBank;
00439 }
00440
00441 ScanMatcher* ScanMatcher::Create(OpenMapper* pOpenMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold)
00442 {
00443
00444 if (resolution <= 0)
00445 {
00446 return NULL;
00447 }
00448 if (searchSize <= 0)
00449 {
00450 return NULL;
00451 }
00452 if (smearDeviation < 0)
00453 {
00454 return NULL;
00455 }
00456 if (rangeThreshold <= 0)
00457 {
00458 return NULL;
00459 }
00460
00461 assert(math::DoubleEqual(math::Round(searchSize / resolution), (searchSize / resolution)));
00462
00463
00464 kt_int32u searchSpaceSideSize = static_cast<kt_int32u>(math::Round(searchSize / resolution) + 1);
00465
00466
00467
00468 kt_int32u pointReadingMargin = static_cast<kt_int32u>(ceil(rangeThreshold / resolution));
00469
00470 kt_int32s gridSize = searchSpaceSideSize + 2 * pointReadingMargin;
00471
00472
00473 assert(gridSize % 2 == 1);
00474 CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation);
00475
00476
00477 Grid<kt_double>* pSearchSpaceProbs = Grid<kt_double>::CreateGrid(searchSpaceSideSize, searchSpaceSideSize, resolution);
00478
00479 GridIndexLookup<kt_int8u>* pGridLookup = new GridIndexLookup<kt_int8u>(pCorrelationGrid);
00480
00481 ScanMatcher* pScanMatcher = new ScanMatcher(pOpenMapper);
00482 pScanMatcher->m_pScanMatcherGridSet = new ScanMatcherGridSet(pCorrelationGrid, pSearchSpaceProbs, pGridLookup);
00483
00484 if (pOpenMapper->IsMultiThreaded())
00485 {
00486 #ifdef USE_TBB
00487 pScanMatcher->m_pScanMatcherGridSetBank = new ScanMatcherGridSetBank(10, gridSize, gridSize, resolution, smearDeviation, searchSpaceSideSize, searchSpaceSideSize);
00488 #else
00489 pScanMatcher->m_pScanMatcherGridSetBank = NULL;
00490 #endif
00491 }
00492
00493 return pScanMatcher;
00494 }
00495
00496 kt_double ScanMatcher::MatchScan(LocalizedLaserScan* pScan, const LocalizedLaserScanList& rBaseScans, Pose2& rMean, Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch)
00497 {
00498 SmartPointer<ScanMatcherGridSet> pScanMatcherGridSet;
00499
00500 if (m_pOpenMapper->IsMultiThreaded())
00501 {
00502 #ifdef USE_TBB
00503 pScanMatcherGridSet = m_pScanMatcherGridSetBank->CheckOut();
00504 #else
00505 pScanMatcherGridSet = m_pScanMatcherGridSet;
00506 #endif
00507 }
00508 else
00509 {
00510 pScanMatcherGridSet = m_pScanMatcherGridSet;
00511 }
00512
00513 CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
00514 Grid<kt_double>* pSearchSpaceProbs = pScanMatcherGridSet->m_pSearchSpaceProbs;
00515
00517
00518
00519
00520 Pose2 scanPose = pScan->GetSensorPose();
00521
00522
00523
00524 if (pScan->GetPointReadings(true).Size() == 0)
00525 {
00526 rMean = scanPose;
00527
00528
00529 rCovariance(0, 0) = MAX_VARIANCE;
00530 rCovariance(1, 1) = MAX_VARIANCE;
00531 rCovariance(2, 2) = 4 * math::Square(m_pOpenMapper->m_pCoarseAngleResolution->GetValue());
00532
00533 if (m_pOpenMapper->IsMultiThreaded())
00534 {
00535 #ifdef USE_TBB
00536 m_pScanMatcherGridSetBank->Return(pScanMatcherGridSet);
00537 #endif
00538 }
00539
00540 return 0.0;
00541 }
00542
00543
00544 Rectangle2<kt_int32s> roi = pCorrelationGrid->GetROI();
00545
00546
00547 Vector2d offset;
00548 offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * pCorrelationGrid->GetResolution()));
00549 offset.SetY(scanPose.GetY() - (0.5 * (roi.GetHeight() - 1) * pCorrelationGrid->GetResolution()));
00550
00551
00552 pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset);
00553
00555
00556
00557 AddScansNew(pCorrelationGrid, rBaseScans, scanPose.GetPosition());
00558
00559
00560 Vector2d searchDimensions(pSearchSpaceProbs->GetWidth(), pSearchSpaceProbs->GetHeight());
00561 Vector2d coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * pCorrelationGrid->GetResolution(), 0.5 * (searchDimensions.GetY() - 1) * pCorrelationGrid->GetResolution());
00562
00563
00564 Vector2d coarseSearchResolution(2 * pCorrelationGrid->GetResolution(), 2 * pCorrelationGrid->GetResolution());
00565
00566
00567 kt_double bestResponse = CorrelateScan(pScanMatcherGridSet, pScan, scanPose, coarseSearchOffset, coarseSearchResolution, m_pOpenMapper->m_pCoarseSearchAngleOffset->GetValue(), m_pOpenMapper->m_pCoarseAngleResolution->GetValue(), doPenalize, rMean, rCovariance, false);
00568
00569 if (m_pOpenMapper->m_pUseResponseExpansion->GetValue() == true)
00570 {
00571 if (math::DoubleEqual(bestResponse, 0.0))
00572 {
00573 #ifdef KARTO_DEBUG
00574 std::cout << "Mapper Info: Expanding response search space!" << std::endl;
00575 #endif
00576
00577 kt_double newSearchAngleOffset = m_pOpenMapper->m_pCoarseSearchAngleOffset->GetValue();
00578 for (kt_int32u i = 0; i < 3; i++)
00579 {
00580 newSearchAngleOffset += math::DegreesToRadians(20);
00581
00582 bestResponse = CorrelateScan(pScanMatcherGridSet, pScan, scanPose, coarseSearchOffset, coarseSearchResolution, newSearchAngleOffset, m_pOpenMapper->m_pCoarseAngleResolution->GetValue(), doPenalize, rMean, rCovariance, false);
00583
00584 if (math::DoubleEqual(bestResponse, 0.0) == false)
00585 {
00586 break;
00587 }
00588 }
00589
00590 #ifdef KARTO_DEBUG
00591 if (math::DoubleEqual(bestResponse, 0.0))
00592 {
00593 std::cout << "Mapper Warning: Unable to calculate response!" << std::endl;
00594 }
00595 #endif
00596 }
00597 }
00598
00599 if (doRefineMatch)
00600 {
00601 Vector2d fineSearchOffset(coarseSearchResolution * 0.5);
00602 Vector2d fineSearchResolution(pCorrelationGrid->GetResolution(), pCorrelationGrid->GetResolution());
00603 bestResponse = CorrelateScan(pScanMatcherGridSet, pScan, rMean, fineSearchOffset, fineSearchResolution, 0.5 * m_pOpenMapper->m_pCoarseAngleResolution->GetValue(), m_pOpenMapper->m_pFineSearchAngleOffset->GetValue(), doPenalize, rMean, rCovariance, true);
00604 }
00605
00606 #ifdef KARTO_DEBUG
00607 std::cout << " BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ", VARIANCE = " << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl;
00608 #endif
00609
00610 assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
00611
00612 if (m_pOpenMapper->IsMultiThreaded())
00613 {
00614 #ifdef USE_TBB
00615 m_pScanMatcherGridSetBank->Return(pScanMatcherGridSet);
00616 #endif
00617 }
00618
00619 return bestResponse;
00620 }
00621
00622 #ifdef USE_TBB
00623 class Parallel_CorrelateScan
00624 {
00625 public:
00626 Parallel_CorrelateScan(std::vector<kt_double>* pNewPositionsY, std::vector<kt_double>* pSquaresY,
00627 std::vector<kt_double>* pNewPositionsX, std::vector<kt_double>* pSquaresX,
00628 std::vector<kt_double>* pAngles,
00629 std::vector<std::pair<kt_double, Pose2> >* pPoseResponses,
00630 ScanMatcher* pScanMatcher, kt_bool doPenalize,
00631 kt_double distanceVariancePenalty, kt_double minimumDistancePenalty,
00632 kt_double angleVariancePenalty, kt_double minimumAnglePenalty,
00633 kt_double searchCenterHeading, ScanMatcherGridSet* pScanMatcherGridSet)
00634 : m_pNewPositionsY(pNewPositionsY)
00635 , m_pSquaresY(pSquaresY)
00636 , m_pNewPositionsX(pNewPositionsX)
00637 , m_pSquaresX(pSquaresX)
00638 , m_pAngles(pAngles)
00639 , m_pPoseResponses(pPoseResponses)
00640 , m_pScanMatcher(pScanMatcher)
00641 , m_DoPenalize(doPenalize)
00642 , m_DistanceVariancePenalty(distanceVariancePenalty)
00643 , m_MinimumDistancePenalty(minimumDistancePenalty)
00644 , m_AngleVariancePenalty(angleVariancePenalty)
00645 , m_MinimumAnglePenalty(minimumAnglePenalty)
00646 , m_SearchCenterHeading(searchCenterHeading)
00647 , m_pScanMatcherGridSet(pScanMatcherGridSet)
00648 {
00649 m_nX = pNewPositionsX->size();
00650 m_nAngles = pAngles->size();
00651 }
00652
00653 void operator()(const tbb::blocked_range3d<kt_int32s, kt_int32s, kt_int32s>& rRange) const
00654 {
00655 CorrelationGrid* pCorrelationGrid = m_pScanMatcherGridSet->m_pCorrelationGrid;
00656
00657 for (tbb::blocked_range<kt_int32s>::const_iterator yIndex = rRange.pages().begin(); yIndex != rRange.pages().end(); yIndex++)
00658 {
00659 kt_double newPositionY = m_pNewPositionsY->at(yIndex);
00660 kt_double squareY = m_pSquaresY->at(yIndex);
00661
00662 for (tbb::blocked_range<kt_int32s>::const_iterator xIndex = rRange.rows().begin(); xIndex != rRange.rows().end(); xIndex++)
00663 {
00664 kt_double newPositionX = m_pNewPositionsX->at(xIndex);
00665 kt_double squareX = m_pSquaresX->at(xIndex);
00666
00667 Vector2i gridPoint = pCorrelationGrid->WorldToGrid(Vector2d(newPositionX, newPositionY));
00668 kt_int32s gridIndex = pCorrelationGrid->GridIndex(gridPoint);
00669 assert(gridIndex >= 0);
00670
00671 kt_double squaredDistance = squareX + squareY;
00672
00673 for (tbb::blocked_range<kt_int32s>::const_iterator angleIndex = rRange.cols().begin(); angleIndex != rRange.cols().end(); angleIndex++)
00674 {
00675 kt_int32u poseResponseIndex = (m_nX * m_nAngles) * yIndex + m_nAngles * xIndex + angleIndex;
00676
00677 kt_double angle = m_pAngles->at(angleIndex);
00678
00679 kt_double response = m_pScanMatcher->GetResponse(m_pScanMatcherGridSet, angleIndex, gridIndex);
00680 if (m_DoPenalize && (math::DoubleEqual(response, 0.0) == false))
00681 {
00682
00683
00684 kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * squaredDistance / m_DistanceVariancePenalty);
00685 distancePenalty = math::Maximum(distancePenalty, m_MinimumDistancePenalty);
00686
00687 kt_double squaredAngleDistance = math::Square(angle - m_SearchCenterHeading);
00688 kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * squaredAngleDistance / m_AngleVariancePenalty);
00689 anglePenalty = math::Maximum(anglePenalty, m_MinimumAnglePenalty);
00690
00691 response *= (distancePenalty * anglePenalty);
00692 }
00693
00694
00695 m_pPoseResponses->at(poseResponseIndex) = std::pair<kt_double, Pose2>(response, Pose2(newPositionX, newPositionY, math::NormalizeAngle(angle)));
00696 }
00697 }
00698 }
00699 }
00700
00701 private:
00702 std::vector<kt_double>* m_pNewPositionsY;
00703 std::vector<kt_double>* m_pSquaresY;
00704 std::vector<kt_double>* m_pNewPositionsX;
00705 std::vector<kt_double>* m_pSquaresX;
00706 std::vector<kt_double>* m_pAngles;
00707 std::vector<std::pair<kt_double, Pose2> >* m_pPoseResponses;
00708 ScanMatcher* m_pScanMatcher;
00709 kt_bool m_DoPenalize;
00710 kt_double m_DistanceVariancePenalty, m_MinimumDistancePenalty;
00711 kt_double m_AngleVariancePenalty, m_MinimumAnglePenalty;
00712 kt_double m_SearchCenterHeading;
00713 kt_int32u m_nX, m_nAngles;
00714 ScanMatcherGridSet* m_pScanMatcherGridSet;
00715 };
00716 #endif
00717
00718 kt_double ScanMatcher::CorrelateScan(ScanMatcherGridSet* pScanMatcherGridSet, LocalizedLaserScan* pScan, const Pose2& rSearchCenter, const Vector2d& rSearchSpaceOffset, const Vector2d& rSearchSpaceResolution,
00719 kt_double searchAngleOffset, kt_double searchAngleResolution, kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch)
00720 {
00721 assert(searchAngleResolution != 0.0);
00722
00723 CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
00724 Grid<kt_double>* pSearchSpaceProbs = pScanMatcherGridSet->m_pSearchSpaceProbs;
00725 GridIndexLookup<kt_int8u>* pGridLookup = pScanMatcherGridSet->m_pGridLookup;
00726
00727
00728 pGridLookup->ComputeOffsets(pScan, rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution);
00729
00730
00731 if (!doingFineMatch)
00732 {
00733 pSearchSpaceProbs->Clear();
00734
00735
00736 Vector2d offset(rSearchCenter.GetPosition() - rSearchSpaceOffset);
00737 pSearchSpaceProbs->GetCoordinateConverter()->SetOffset(offset);
00738 }
00739
00740
00741
00742 kt_int32u nX = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetX() * 2.0 / rSearchSpaceResolution.GetX()) + 1);
00743 std::vector<kt_double> xPoses(nX), newPositionsX(nX), squaresX(nX);
00744 kt_double startX = -rSearchSpaceOffset.GetX();
00745 for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
00746 {
00747 kt_double x = startX + xIndex * rSearchSpaceResolution.GetX();
00748 xPoses[xIndex] = x;
00749 newPositionsX[xIndex] = rSearchCenter.GetX() + x;
00750 squaresX[xIndex] = math::Square(x);
00751 }
00752 assert(math::DoubleEqual(xPoses.back(), -startX));
00753
00754 kt_int32u nY = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetY() * 2.0 / rSearchSpaceResolution.GetY()) + 1);
00755 std::vector<kt_double> yPoses(nY), newPositionsY(nY), squaresY(nY);
00756 kt_double startY = -rSearchSpaceOffset.GetY();
00757 for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
00758 {
00759 kt_double y = startY + yIndex * rSearchSpaceResolution.GetY();
00760 yPoses[yIndex] = y;
00761 newPositionsY[yIndex] = rSearchCenter.GetY() + y;
00762 squaresY[yIndex] = math::Square(y);
00763 }
00764 assert(math::DoubleEqual(yPoses.back(), -startY));
00765
00766
00767 kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1);
00768 std::vector<kt_double> angles(nAngles);
00769 kt_double angle = 0.0;
00770 kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
00771 for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
00772 {
00773 angle = startAngle + angleIndex * searchAngleResolution;
00774 angles[angleIndex] = angle;
00775 }
00776 assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
00777
00778
00779 kt_int32u poseResponseSize = nX * nY * nAngles;
00780 std::vector<std::pair<kt_double, Pose2> > poseResponses = std::vector<std::pair<kt_double, Pose2> >(poseResponseSize);
00781
00782 Vector2i startGridPoint = pCorrelationGrid->WorldToGrid(Vector2d(rSearchCenter.GetX() + startX, rSearchCenter.GetY() + startY));
00783
00784
00785 kt_bool gotTbb = false;
00786 if (m_pOpenMapper->IsMultiThreaded())
00787 {
00788 #ifdef USE_TBB
00789 gotTbb = true;
00790 Parallel_CorrelateScan myTask(&newPositionsY, &squaresY, &newPositionsX, &squaresX, &angles,
00791 &poseResponses, this, doPenalize,
00792 m_pOpenMapper->m_pDistanceVariancePenalty->GetValue(),
00793 m_pOpenMapper->m_pMinimumDistancePenalty->GetValue(),
00794 m_pOpenMapper->m_pAngleVariancePenalty->GetValue(),
00795 m_pOpenMapper->m_pMinimumAnglePenalty->GetValue(),
00796 rSearchCenter.GetHeading(), pScanMatcherGridSet);
00797 int grainSizeY = 10;
00798 int grainSizeX = 10;
00799 int grainSizeAngle = 10;
00800 tbb::parallel_for(tbb::blocked_range3d<kt_int32s>(0, static_cast<kt_int32s>(nY), grainSizeY, 0, static_cast<kt_int32s>(nX), grainSizeX, 0, nAngles, grainSizeAngle), myTask);
00801 #endif
00802 }
00803
00804
00805 if (gotTbb == false)
00806 {
00807 kt_int32u poseResponseCounter = 0;
00808 for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
00809 {
00810 kt_double newPositionY = newPositionsY[yIndex];
00811 kt_double squareY = squaresY[yIndex];
00812
00813 for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
00814 {
00815 kt_double newPositionX = newPositionsX[xIndex];
00816 kt_double squareX = squaresX[xIndex];
00817
00818 Vector2i gridPoint = pCorrelationGrid->WorldToGrid(Vector2d(newPositionX, newPositionY));
00819 kt_int32s gridIndex = pCorrelationGrid->GridIndex(gridPoint);
00820 assert(gridIndex >= 0);
00821
00822 kt_double squaredDistance = squareX + squareY;
00823 for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
00824 {
00825 kt_double angle = angles[angleIndex];
00826
00827 kt_double response = GetResponse(pScanMatcherGridSet, angleIndex, gridIndex);
00828 if (doPenalize && (math::DoubleEqual(response, 0.0) == false))
00829 {
00830
00831
00832 kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN * squaredDistance / m_pOpenMapper->m_pDistanceVariancePenalty->GetValue());
00833 distancePenalty = math::Maximum(distancePenalty, m_pOpenMapper->m_pMinimumDistancePenalty->GetValue());
00834
00835 kt_double squaredAngleDistance = math::Square(angle - rSearchCenter.GetHeading());
00836 kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN * squaredAngleDistance / m_pOpenMapper->m_pAngleVariancePenalty->GetValue());
00837 anglePenalty = math::Maximum(anglePenalty, m_pOpenMapper->m_pMinimumAnglePenalty->GetValue());
00838
00839 response *= (distancePenalty * anglePenalty);
00840 }
00841
00842
00843 poseResponses[poseResponseCounter] = std::pair<kt_double, Pose2>(response, Pose2(newPositionX, newPositionY, math::NormalizeAngle(angle)));
00844 poseResponseCounter++;
00845 }
00846 }
00847 }
00848
00849 assert(poseResponseSize == poseResponseCounter);
00850 }
00851
00852
00853 kt_double bestResponse = -1;
00854 for (kt_int32u i = 0; i < poseResponseSize; i++)
00855 {
00856 bestResponse = math::Maximum(bestResponse, poseResponses[i].first);
00857
00858
00859 if (!doingFineMatch)
00860 {
00861 const Pose2& rPose = poseResponses[i].second;
00862 Vector2i grid = pSearchSpaceProbs->WorldToGrid(rPose.GetPosition());
00863
00864 kt_double* ptr = (kt_double*)pSearchSpaceProbs->GetDataPointer(grid);
00865 if (ptr == NULL)
00866 {
00867 throw Exception("Mapper FATAL ERROR - Index out of range in probability search!");
00868 }
00869
00870 *ptr = math::Maximum(poseResponses[i].first, *ptr);
00871 }
00872 }
00873
00874
00875 Vector2d averagePosition;
00876 kt_double thetaX = 0.0;
00877 kt_double thetaY = 0.0;
00878 kt_int32s averagePoseCount = 0;
00879 for (kt_int32u i = 0; i < poseResponseSize; i++)
00880 {
00881 if (math::DoubleEqual(poseResponses[i].first, bestResponse))
00882 {
00883 averagePosition += poseResponses[i].second.GetPosition();
00884
00885 kt_double heading = poseResponses[i].second.GetHeading();
00886 thetaX += cos(heading);
00887 thetaY += sin(heading);
00888
00889 averagePoseCount++;
00890 }
00891 }
00892
00893 Pose2 averagePose;
00894 if (averagePoseCount > 0)
00895 {
00896 averagePosition /= averagePoseCount;
00897
00898 thetaX /= averagePoseCount;
00899 thetaY /= averagePoseCount;
00900
00901 averagePose = Pose2(averagePosition, atan2(thetaY, thetaX));
00902 }
00903 else
00904 {
00905 throw Exception("Mapper FATAL ERROR - Unable to find best position");
00906 }
00907
00908 #ifdef KARTO_DEBUG
00909 std::cout << "bestPose: " << averagePose << std::endl;
00910 std::cout << "bestResponse: " << bestResponse << std::endl;
00911 #endif
00912
00913 if (!doingFineMatch)
00914 {
00915 ComputePositionalCovariance(pSearchSpaceProbs, averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset, rSearchSpaceResolution, searchAngleResolution, rCovariance);
00916 }
00917 else
00918 {
00919 ComputeAngularCovariance(pScanMatcherGridSet, averagePose, bestResponse, rSearchCenter, searchAngleOffset, searchAngleResolution, rCovariance);
00920 }
00921
00922 rMean = averagePose;
00923
00924 #ifdef KARTO_DEBUG
00925 std::cout << "bestPose: " << averagePose << std::endl;
00926 #endif
00927
00928 if (bestResponse > 1.0)
00929 {
00930 bestResponse = 1.0;
00931 }
00932
00933 assert(math::InRange(bestResponse, 0.0, 1.0));
00934 assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
00935
00936 return bestResponse;
00937 }
00938
00939 void ScanMatcher::ComputePositionalCovariance(Grid<kt_double>* pSearchSpaceProbs, const Pose2& rBestPose, kt_double bestResponse,
00940 const Pose2& rSearchCenter, const Vector2d& rSearchSpaceOffset,
00941 const Vector2d& rSearchSpaceResolution, kt_double searchAngleResolution, Matrix3& rCovariance)
00942 {
00943
00944 rCovariance.SetToIdentity();
00945
00946
00947 if (bestResponse < KT_TOLERANCE)
00948 {
00949 rCovariance(0, 0) = MAX_VARIANCE;
00950 rCovariance(1, 1) = MAX_VARIANCE;
00951 rCovariance(2, 2) = 4 * math::Square(searchAngleResolution);
00952
00953 return;
00954 }
00955
00956 kt_double accumulatedVarianceXX = 0;
00957 kt_double accumulatedVarianceXY = 0;
00958 kt_double accumulatedVarianceYY = 0;
00959 kt_double norm = 0;
00960
00961 kt_double dx = rBestPose.GetX() - rSearchCenter.GetX();
00962 kt_double dy = rBestPose.GetY() - rSearchCenter.GetY();
00963
00964 kt_double offsetX = rSearchSpaceOffset.GetX();
00965 kt_double offsetY = rSearchSpaceOffset.GetY();
00966
00967 kt_int32u nX = static_cast<kt_int32u>(math::Round(offsetX * 2.0 / rSearchSpaceResolution.GetX()) + 1);
00968 kt_double startX = -offsetX;
00969 assert(math::DoubleEqual(startX + (nX - 1) * rSearchSpaceResolution.GetX(), -startX));
00970
00971 kt_int32u nY = static_cast<kt_int32u>(math::Round(offsetY * 2.0 / rSearchSpaceResolution.GetY()) + 1);
00972 kt_double startY = -offsetY;
00973 assert(math::DoubleEqual(startY + (nY - 1) * rSearchSpaceResolution.GetY(), -startY));
00974
00975 for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
00976 {
00977 kt_double y = startY + yIndex * rSearchSpaceResolution.GetY();
00978
00979 for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
00980 {
00981 kt_double x = startX + xIndex * rSearchSpaceResolution.GetX();
00982
00983 Vector2i gridPoint = pSearchSpaceProbs->WorldToGrid(Vector2d(rSearchCenter.GetX() + x, rSearchCenter.GetY() + y));
00984 kt_double response = *(pSearchSpaceProbs->GetDataPointer(gridPoint));
00985
00986
00987 if (response >= (bestResponse - 0.1))
00988 {
00989 norm += response;
00990 accumulatedVarianceXX += (math::Square(x - dx) * response);
00991 accumulatedVarianceXY += ((x - dx) * (y - dy) * response);
00992 accumulatedVarianceYY += (math::Square(y - dy) * response);
00993 }
00994 }
00995 }
00996
00997 if (norm > KT_TOLERANCE)
00998 {
00999 kt_double varianceXX = accumulatedVarianceXX / norm;
01000 kt_double varianceXY = accumulatedVarianceXY / norm;
01001 kt_double varianceYY = accumulatedVarianceYY / norm;
01002 kt_double varianceTHTH = 4 * math::Square(searchAngleResolution);
01003
01004
01005
01006 kt_double minVarianceXX = 0.1 * math::Square(rSearchSpaceResolution.GetX());
01007 kt_double minVarianceYY = 0.1 * math::Square(rSearchSpaceResolution.GetY());
01008 varianceXX = math::Maximum(varianceXX, minVarianceXX);
01009 varianceYY = math::Maximum(varianceYY, minVarianceYY);
01010
01011
01012 kt_double multiplier = 1.0 / bestResponse;
01013 rCovariance(0, 0) = varianceXX * multiplier;
01014 rCovariance(0, 1) = varianceXY * multiplier;
01015 rCovariance(1, 0) = varianceXY * multiplier;
01016 rCovariance(1, 1) = varianceYY * multiplier;
01017 rCovariance(2, 2) = varianceTHTH;
01018 }
01019
01020
01021
01022 if (math::DoubleEqual(rCovariance(0, 0), 0.0))
01023 {
01024 rCovariance(0, 0) = MAX_VARIANCE;
01025 }
01026
01027 if (math::DoubleEqual(rCovariance(1, 1), 0.0))
01028 {
01029 rCovariance(1, 1) = MAX_VARIANCE;
01030 }
01031 }
01032
01033 void ScanMatcher::ComputeAngularCovariance(ScanMatcherGridSet* pScanMatcherGridSet, const Pose2& rBestPose, kt_double bestResponse, const Pose2& rSearchCenter,
01034 kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3& rCovariance)
01035 {
01036
01037
01038 CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
01039
01040
01041 kt_double bestAngle = math::NormalizeAngleDifference(rBestPose.GetHeading(), rSearchCenter.GetHeading());
01042
01043 Vector2i gridPoint = pCorrelationGrid->WorldToGrid(rBestPose.GetPosition());
01044 kt_int32s gridIndex = pCorrelationGrid->GridIndex(gridPoint);
01045
01046 kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2 / searchAngleResolution) + 1);
01047
01048 kt_double angle = 0.0;
01049 kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
01050
01051 kt_double norm = 0.0;
01052 kt_double accumulatedVarianceThTh = 0.0;
01053 for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
01054 {
01055 angle = startAngle + angleIndex * searchAngleResolution;
01056 kt_double response = GetResponse(pScanMatcherGridSet, angleIndex, gridIndex);
01057
01058
01059 if (response >= (bestResponse - 0.1))
01060 {
01061 norm += response;
01062 accumulatedVarianceThTh += (math::Square(angle - bestAngle) * response);
01063 }
01064 }
01065 assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
01066
01067 if (norm > KT_TOLERANCE)
01068 {
01069 if (accumulatedVarianceThTh < KT_TOLERANCE)
01070 {
01071 accumulatedVarianceThTh = math::Square(searchAngleResolution);
01072 }
01073
01074 accumulatedVarianceThTh /= norm;
01075 }
01076 else
01077 {
01078 accumulatedVarianceThTh = 1000 * math::Square(searchAngleResolution);
01079 }
01080
01081 rCovariance(2, 2) = accumulatedVarianceThTh;
01082 }
01083
01084 void ScanMatcher::AddScans(CorrelationGrid* pCorrelationGrid, const LocalizedLaserScanList& rScans, const Vector2d& rViewPoint)
01085 {
01086 pCorrelationGrid->Clear();
01087
01088
01089 karto_const_forEach(LocalizedLaserScanList, &rScans)
01090 {
01091 AddScan(pCorrelationGrid, *iter, rViewPoint);
01092 }
01093 }
01094
01095 void ScanMatcher::AddScansNew(CorrelationGrid* pCorrelationGrid, const LocalizedLaserScanList& rScans, const Vector2d& rViewPoint)
01096 {
01097 pCorrelationGrid->Clear();
01098
01099 kt_int32s index = 0;
01100 kt_size_t nScans = rScans.Size();
01101 Vector2dList* pValidPoints = new Vector2dList[nScans];
01102
01103
01104
01105
01106
01107
01108
01109
01110 karto_const_forEach(LocalizedLaserScanList, &rScans)
01111 {
01112 pValidPoints[index++] = FindValidPoints(*iter, rViewPoint);
01113 }
01114
01115
01116 for (kt_size_t i = 0; i < nScans; i++)
01117 {
01118 AddScanNew(pCorrelationGrid, pValidPoints[i]);
01119 }
01120
01121 delete[] pValidPoints;
01122 }
01123
01124 void ScanMatcher::AddScan(CorrelationGrid* pCorrelationGrid, LocalizedLaserScan* pScan, const Vector2d& rViewPoint, kt_bool doSmear)
01125 {
01126 Vector2dList validPoints = FindValidPoints(pScan, rViewPoint);
01127
01128
01129 karto_const_forEach(Vector2dList, &validPoints)
01130 {
01131 Vector2i gridPoint = pCorrelationGrid->WorldToGrid(*iter);
01132 if (!math::IsUpTo(gridPoint.GetX(), pCorrelationGrid->GetROI().GetWidth()) || !math::IsUpTo(gridPoint.GetY(), pCorrelationGrid->GetROI().GetHeight()))
01133 {
01134
01135 continue;
01136 }
01137
01138 int gridIndex = pCorrelationGrid->GridIndex(gridPoint);
01139
01140
01141 if (pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied)
01142 {
01143
01144 continue;
01145 }
01146
01147 pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied;
01148
01149
01150 if (doSmear == true)
01151 {
01152 pCorrelationGrid->SmearPoint(gridPoint);
01153 }
01154 }
01155 }
01156
01157 void ScanMatcher::AddScanNew(CorrelationGrid* pCorrelationGrid, const Vector2dList& rValidPoints, kt_bool doSmear)
01158 {
01159
01160 karto_const_forEach(Vector2dList, &rValidPoints)
01161 {
01162 Vector2i gridPoint = pCorrelationGrid->WorldToGrid(*iter);
01163 if (!math::IsUpTo(gridPoint.GetX(), pCorrelationGrid->GetROI().GetWidth()) || !math::IsUpTo(gridPoint.GetY(), pCorrelationGrid->GetROI().GetHeight()))
01164 {
01165
01166 continue;
01167 }
01168
01169 int gridIndex = pCorrelationGrid->GridIndex(gridPoint);
01170
01171
01172 if (pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied)
01173 {
01174
01175 continue;
01176 }
01177
01178 pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied;
01179
01180
01181 if (doSmear == true)
01182 {
01183 pCorrelationGrid->SmearPoint(gridPoint);
01184 }
01185 }
01186 }
01187
01188 Vector2dList ScanMatcher::FindValidPoints(LocalizedLaserScan* pScan, const Vector2d& rViewPoint)
01189 {
01190 const Vector2dList& rPointReadings = pScan->GetPointReadings(true);
01191
01192
01193 const kt_double minSquareDistance = math::Square(0.1);
01194
01195
01196
01197 Vector2dList::ConstIterator trailingPointIter = rPointReadings.GetConstIterator();
01198 Vector2dList validPoints;
01199
01200 Vector2d firstPoint;
01201 kt_bool firstTime = true;
01202 karto_const_forEach(Vector2dList, &rPointReadings)
01203 {
01204 Vector2d currentPoint = *iter;
01205
01206 if (firstTime)
01207 {
01208 firstPoint = currentPoint;
01209 firstTime = false;
01210 }
01211
01212 Vector2d delta = firstPoint - currentPoint;
01213 if (delta.SquaredLength() > minSquareDistance)
01214 {
01215
01216
01217
01218
01219
01220 double a = rViewPoint.GetY() - firstPoint.GetY();
01221 double b = firstPoint.GetX() - rViewPoint.GetX();
01222 double c = firstPoint.GetY() * rViewPoint.GetX() - firstPoint.GetX() * rViewPoint.GetY();
01223 double ss = currentPoint.GetX() * a + currentPoint.GetY() * b + c;
01224
01225
01226 firstPoint = currentPoint;
01227
01228 if (ss < 0.0)
01229 {
01230 trailingPointIter = iter;
01231 }
01232 else
01233 {
01234 for (; trailingPointIter != iter; trailingPointIter++)
01235 {
01236 validPoints.Add(*trailingPointIter);
01237 }
01238 }
01239 }
01240 }
01241
01242 return validPoints;
01243 }
01244
01245 kt_double ScanMatcher::GetResponse(ScanMatcherGridSet* pScanMatcherGridSet, kt_int32u angleIndex, kt_int32s gridPositionIndex)
01246 {
01247 CorrelationGrid* pCorrelationGrid = pScanMatcherGridSet->m_pCorrelationGrid;
01248 GridIndexLookup<kt_int8u>* pGridLookup = pScanMatcherGridSet->m_pGridLookup;
01249
01250 kt_double response = 0.0;
01251
01252
01253 kt_int8u* pByte = pCorrelationGrid->GetDataPointer() + gridPositionIndex;
01254
01255 const LookupArray* pOffsets = pGridLookup->GetLookupArray(angleIndex);
01256 assert(pOffsets != NULL);
01257
01258
01259 kt_int32u nPoints = pOffsets->GetSize();
01260 if (nPoints == 0)
01261 {
01262 return response;
01263 }
01264
01265
01266 kt_int32s* pAngleIndexPointer = pOffsets->GetArrayPointer();
01267 for (kt_int32u i = 0; i < nPoints; i++)
01268 {
01269
01270 kt_int32s pointGridIndex = gridPositionIndex + pAngleIndexPointer[i];
01271 if (!math::IsUpTo(pointGridIndex, pCorrelationGrid->GetDataSize()))
01272 {
01273 continue;
01274 }
01275
01276
01277 response += pByte[pAngleIndexPointer[i]];
01278 }
01279
01280
01281 response /= (nPoints * GridStates_Occupied);
01282 assert(fabs(response) <= 1.0);
01283
01284 return response;
01285 }
01286
01287 CorrelationGrid* ScanMatcher::GetCorrelationGrid() const
01288 {
01289 if (m_pOpenMapper->IsMultiThreaded())
01290 {
01291 throw Exception("Correlation grid only available in single-threaded mode");
01292 }
01293 else
01294 {
01295 return m_pScanMatcherGridSet->m_pCorrelationGrid;
01296 }
01297 }
01298
01299 Grid<kt_double>* ScanMatcher::GetSearchGrid() const
01300 {
01301 if (m_pOpenMapper->IsMultiThreaded())
01302 {
01303 throw Exception("Search grid only available in single-threaded mode");
01304 }
01305 else
01306 {
01307 return m_pScanMatcherGridSet->m_pSearchSpaceProbs;
01308 }
01309 }
01310
01314
01315 template<typename T>
01316 class BreadthFirstTraversal : public GraphTraversal<T>
01317 {
01318 public:
01322 BreadthFirstTraversal(Graph<T>* pGraph)
01323 : GraphTraversal<T>(pGraph)
01324 {
01325 }
01326
01330 virtual ~BreadthFirstTraversal()
01331 {
01332 }
01333
01334 public:
01341 virtual List<T> Traverse(Vertex<T>* pStartVertex, Visitor<T>* pVisitor)
01342 {
01343 std::queue<Vertex<T>*> toVisit;
01344 std::set<Vertex<T>*> seenVertices;
01345 std::vector<Vertex<T>*> validVertices;
01346
01347 toVisit.push(pStartVertex);
01348 seenVertices.insert(pStartVertex);
01349
01350 do
01351 {
01352 Vertex<T>* pNext = toVisit.front();
01353 toVisit.pop();
01354
01355 if (pVisitor->Visit(pNext))
01356 {
01357
01358 validVertices.push_back(pNext);
01359
01360 List<Vertex<T>*> adjacentVertices = pNext->GetAdjacentVertices();
01361 karto_const_forEach(typename List<Vertex<T>*>, &adjacentVertices)
01362 {
01363 Vertex<T>* pAdjacent = *iter;
01364
01365
01366 if (seenVertices.find(pAdjacent) == seenVertices.end())
01367 {
01368 toVisit.push(pAdjacent);
01369 seenVertices.insert(pAdjacent);
01370 }
01371 }
01372 }
01373 } while (toVisit.empty() == false);
01374
01375 List<T> objects;
01376 forEach(typename std::vector<Vertex<T>*>, &validVertices)
01377 {
01378 objects.Add((*iter)->GetVertexObject());
01379 }
01380
01381 return objects;
01382 }
01383
01384 };
01385
01389
01390 class NearScanVisitor : public Visitor<LocalizedObjectPtr>
01391 {
01392 public:
01393 NearScanVisitor(LocalizedLaserScan* pScan, kt_double maxDistance, kt_bool useScanBarycenter)
01394 : m_MaxDistanceSquared(math::Square(maxDistance))
01395 , m_UseScanBarycenter(useScanBarycenter)
01396 {
01397 m_CenterPose = pScan->GetReferencePose(m_UseScanBarycenter);
01398 }
01399
01400 virtual kt_bool Visit(Vertex<LocalizedObjectPtr>* pVertex)
01401 {
01402 LocalizedObject* pObject = pVertex->GetVertexObject();
01403
01404 LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
01405
01406
01407 if (pScan == NULL)
01408 {
01409 return false;
01410 }
01411
01412 Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter);
01413
01414 kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition());
01415 return (squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE);
01416 }
01417
01418 protected:
01419 Pose2 m_CenterPose;
01420 kt_double m_MaxDistanceSquared;
01421 kt_bool m_UseScanBarycenter;
01422
01423 };
01424
01428
01429 MapperGraph::MapperGraph(OpenMapper* pOpenMapper, kt_double rangeThreshold)
01430 : m_pOpenMapper(pOpenMapper)
01431 {
01432 m_pLoopScanMatcher = ScanMatcher::Create(pOpenMapper, m_pOpenMapper->m_pLoopSearchSpaceDimension->GetValue(), m_pOpenMapper->m_pLoopSearchSpaceResolution->GetValue(), m_pOpenMapper->m_pLoopSearchSpaceSmearDeviation->GetValue(), rangeThreshold);
01433 assert(m_pLoopScanMatcher);
01434
01435 m_pTraversal = new BreadthFirstTraversal<LocalizedObjectPtr>(this);
01436 }
01437
01438 MapperGraph::~MapperGraph()
01439 {
01440 delete m_pLoopScanMatcher;
01441 m_pLoopScanMatcher = NULL;
01442
01443 delete m_pTraversal;
01444 m_pTraversal = NULL;
01445 }
01446
01447 void MapperGraph::AddVertex(LocalizedObject* pObject)
01448 {
01449 assert(pObject);
01450
01451 if (pObject == NULL)
01452 {
01453 return;
01454 }
01455
01456 Vertex<LocalizedObjectPtr>* pVertex = new Vertex<LocalizedObjectPtr>(pObject);
01457 Graph<LocalizedObjectPtr>::AddVertex(pVertex);
01458 if (m_pOpenMapper->m_pScanSolver != NULL)
01459 {
01460 m_pOpenMapper->m_pScanSolver->AddNode(pVertex);
01461 }
01462 }
01463
01464 void MapperGraph::AddEdges(LocalizedObject* pObject)
01465 {
01466
01467 Matrix3 covariance;
01468 covariance(0, 0) = MAX_VARIANCE;
01469 covariance(1, 1) = MAX_VARIANCE;
01470 covariance(2, 2) = MAX_VARIANCE;
01471
01472 LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
01473 if (pScan != NULL)
01474 {
01475 AddEdges(pScan, covariance);
01476 }
01477 else
01478 {
01479 MapperSensorManager* pSensorManager = m_pOpenMapper->m_pMapperSensorManager;
01480 const Identifier& rSensorName = pObject->GetSensorIdentifier();
01481
01482 LocalizedLaserScan* pLastScan = pSensorManager->GetLastScan(rSensorName);
01483 if (pLastScan != NULL)
01484 {
01485 LinkObjects(pLastScan, pObject, pObject->GetCorrectedPose(), covariance);
01486 }
01487 }
01488 }
01489
01490 void MapperGraph::AddEdges(LocalizedLaserScan* pScan, const Matrix3& rCovariance)
01491 {
01492 MapperSensorManager* pSensorManager = m_pOpenMapper->m_pMapperSensorManager;
01493
01494 const Identifier& rSensorName = pScan->GetSensorIdentifier();
01495
01496 Pose2List means;
01497 List<Matrix3> covariances;
01498
01499 LocalizedLaserScan* pLastScan = pSensorManager->GetLastScan(rSensorName);
01500 if (pLastScan == NULL)
01501 {
01502
01503
01504 assert(pSensorManager->GetScans(rSensorName).Size() == 1);
01505
01506 List<Identifier> sensorNames = pSensorManager->GetSensorNames();
01507 karto_const_forEach(List<Identifier>, &sensorNames)
01508 {
01509 const Identifier& rCandidateSensorName = *iter;
01510
01511
01512 if ((rCandidateSensorName == rSensorName) || (pSensorManager->GetScans(rCandidateSensorName).IsEmpty()))
01513 {
01514 continue;
01515 }
01516
01517 Pose2 bestPose;
01518 Matrix3 covariance;
01519 kt_double response = m_pOpenMapper->m_pSequentialScanMatcher->MatchScan(pScan, pSensorManager->GetScans(rCandidateSensorName), bestPose, covariance);
01520 LinkObjects(pSensorManager->GetScans(rCandidateSensorName)[0], pScan, bestPose, covariance);
01521
01522
01523 if (response > m_pOpenMapper->m_pLinkMatchMinimumResponseFine->GetValue())
01524 {
01525 means.Add(bestPose);
01526 covariances.Add(covariance);
01527 }
01528 }
01529 }
01530 else
01531 {
01532
01533 LinkObjects(pLastScan, pScan, pScan->GetSensorPose(), rCovariance);
01534
01535
01536 Pose2 scanPose = pScan->GetSensorPose();
01537 means.Add(scanPose);
01538 covariances.Add(rCovariance);
01539 LinkChainToScan(pSensorManager->GetRunningScans(rSensorName), pScan, scanPose, rCovariance);
01540 }
01541
01542
01543 LinkNearChains(pScan, means, covariances);
01544
01545 if (!means.IsEmpty())
01546 {
01547 pScan->SetSensorPose(ComputeWeightedMean(means, covariances));
01548 }
01549 }
01550
01551 kt_bool MapperGraph::TryCloseLoop(LocalizedLaserScan* pScan, const Identifier& rSensorName)
01552 {
01553 kt_bool loopClosed = false;
01554
01555 kt_int32u scanIndex = 0;
01556
01557 LocalizedLaserScanList candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
01558
01559 while (!candidateChain.IsEmpty())
01560 {
01561 #ifdef KARTO_DEBUG2
01562 std::cout << "Candidate chain for " << pScan->GetStateId() << ": [ ";
01563 karto_const_forEach(LocalizedLaserScanList, &candidateChain)
01564 {
01565 std::cout << (*iter)->GetStateId() << " ";
01566 }
01567 std::cout << "]" << std::endl;
01568 #endif
01569
01570 Pose2 bestPose;
01571 Matrix3 covariance;
01572 kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false, false);
01573
01574 StringBuilder message;
01575 message << "COARSE RESPONSE: " << coarseResponse << " (> " << m_pOpenMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")\n";
01576 message << " var: " << covariance(0, 0) << ", " << covariance(1, 1) << " (< " << m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")";
01577
01578 MapperEventArguments eventArguments(message.ToString());
01579 m_pOpenMapper->Message.Notify(this, eventArguments);
01580
01581 if (((coarseResponse > m_pOpenMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
01582 (covariance(0, 0) < m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
01583 (covariance(1, 1) < m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()))
01584 ||
01585
01586 ((coarseResponse > 0.9 * m_pOpenMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
01587 (covariance(0, 0) < 0.01 * m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
01588 (covariance(1, 1) < 0.01 * m_pOpenMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue())))
01589 {
01590
01591 Pose2 oldPose = pScan->GetSensorPose();
01592
01593 pScan->SetSensorPose(bestPose);
01594 kt_double fineResponse = m_pOpenMapper->m_pSequentialScanMatcher->MatchScan(pScan, candidateChain, bestPose, covariance, false);
01595
01596 message.Clear();
01597 message << "FINE RESPONSE: " << fineResponse << " (>" << m_pOpenMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")";
01598 MapperEventArguments eventArguments(message.ToString());
01599 m_pOpenMapper->Message.Notify(this, eventArguments);
01600
01601 if (fineResponse < m_pOpenMapper->m_pLoopMatchMinimumResponseFine->GetValue())
01602 {
01603
01604 pScan->SetSensorPose(oldPose);
01605
01606 MapperEventArguments eventArguments("REJECTED!");
01607 m_pOpenMapper->Message.Notify(this, eventArguments);
01608 }
01609 else
01610 {
01611 MapperEventArguments eventArguments1("Closing loop...");
01612 m_pOpenMapper->PreLoopClosed.Notify(this, eventArguments1);
01613
01614 pScan->SetSensorPose(bestPose);
01615 LinkChainToScan(candidateChain, pScan, bestPose, covariance);
01616 CorrectPoses();
01617
01618 MapperEventArguments eventArguments2("Loop closed!");
01619 m_pOpenMapper->PostLoopClosed.Notify(this, eventArguments2);
01620
01621 m_pOpenMapper->ScansUpdated.Notify(this, karto::EventArguments::Empty());
01622
01623 loopClosed = true;
01624 }
01625 }
01626
01627 candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
01628 }
01629
01630 return loopClosed;
01631 }
01632
01633 LocalizedLaserScan* MapperGraph::GetClosestScanToPose(const LocalizedLaserScanList& rScans, const Pose2& rPose) const
01634 {
01635 LocalizedLaserScan* pClosestScan = NULL;
01636 kt_double bestSquaredDistance = DBL_MAX;
01637
01638 karto_const_forEach(LocalizedLaserScanList, &rScans)
01639 {
01640 Pose2 scanPose = (*iter)->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01641
01642 kt_double squaredDistance = rPose.GetPosition().SquaredDistance(scanPose.GetPosition());
01643 if (squaredDistance < bestSquaredDistance)
01644 {
01645 bestSquaredDistance = squaredDistance;
01646 pClosestScan = *iter;
01647 }
01648 }
01649
01650 return pClosestScan;
01651 }
01652
01653 Edge<LocalizedObjectPtr>* MapperGraph::AddEdge(LocalizedObject* pSourceObject, LocalizedObject* pTargetObject, kt_bool& rIsNewEdge)
01654 {
01655
01656 assert(pSourceObject->GetUniqueId() < (kt_int32s)m_Vertices.Size());
01657 assert(pTargetObject->GetUniqueId() < (kt_int32s)m_Vertices.Size());
01658
01659 Vertex<LocalizedObjectPtr>* v1 = m_Vertices[pSourceObject->GetUniqueId()];
01660 Vertex<LocalizedObjectPtr>* v2 = m_Vertices[pTargetObject->GetUniqueId()];
01661
01662
01663 karto_const_forEach(List<Edge<LocalizedObjectPtr>*>, &(v1->GetEdges()))
01664 {
01665 Edge<LocalizedObjectPtr>* pEdge = *iter;
01666
01667 if (pEdge->GetTarget() == v2)
01668 {
01669 rIsNewEdge = false;
01670 return pEdge;
01671 }
01672 }
01673
01674 Edge<LocalizedObjectPtr>* pEdge = new Edge<LocalizedObjectPtr>(v1, v2);
01675 Graph<LocalizedObjectPtr>::AddEdge(pEdge);
01676 rIsNewEdge = true;
01677 return pEdge;
01678 }
01679
01680 void MapperGraph::LinkObjects(LocalizedObject* pFromObject, LocalizedObject* pToObject, const Pose2& rMean, const Matrix3& rCovariance)
01681 {
01682 kt_bool isNewEdge = true;
01683 Edge<LocalizedObjectPtr>* pEdge = AddEdge(pFromObject, pToObject, isNewEdge);
01684
01685
01686 if (isNewEdge == true)
01687 {
01688 LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pFromObject);
01689 if (pScan != NULL)
01690 {
01691 pEdge->SetLabel(new LinkInfo(pScan->GetSensorPose(), rMean, rCovariance));
01692 }
01693 else
01694 {
01695 pEdge->SetLabel(new LinkInfo(pFromObject->GetCorrectedPose(), rMean, rCovariance));
01696 }
01697 if (m_pOpenMapper->m_pScanSolver != NULL)
01698 {
01699 m_pOpenMapper->m_pScanSolver->AddConstraint(pEdge);
01700 }
01701 }
01702 }
01703
01704 #ifdef USE_TBB
01705 class Parallel_LinkNearChains
01706 {
01707 public:
01708 Parallel_LinkNearChains(OpenMapper* pMapper, LocalizedLaserScan* pScan, const List<LocalizedLaserScanList>* pChains,
01709 kt_bool* pWasChainLinked, Pose2List* pMeans, List<Matrix3>* pCovariances,
01710 kt_int32u minChainSize, kt_double minResponse)
01711 : m_pOpenMapper(pMapper)
01712 , m_pScan(pScan)
01713 , m_pChains(pChains)
01714 , m_pWasChainLinked(pWasChainLinked)
01715 , m_pMeans(pMeans)
01716 , m_pCovariances(pCovariances)
01717 , m_MinChainSize(minChainSize)
01718 , m_MinResponse(minResponse)
01719 {
01720 }
01721
01722 void operator()(const tbb::blocked_range<kt_int32s>& rRange) const
01723 {
01724 for (kt_int32s i = rRange.begin(); i != rRange.end(); i++)
01725 {
01726 m_pWasChainLinked[i] = false;
01727
01728 const LocalizedLaserScanList& rChain = m_pChains->Get(i);
01729
01730 if (rChain.Size() < m_MinChainSize)
01731 {
01732 continue;
01733 }
01734
01735 Pose2 mean;
01736 Matrix3 covariance;
01737
01738
01739 kt_double response = m_pOpenMapper->GetSequentialScanMatcher()->MatchScan(m_pScan, rChain, mean, covariance, false);
01740 if (response > m_MinResponse - KT_TOLERANCE)
01741 {
01742 m_pWasChainLinked[i] = true;
01743 m_pMeans->Set(i, mean);
01744 m_pCovariances->Set(i, covariance);
01745 }
01746 }
01747 }
01748
01749 private:
01750 OpenMapper* m_pOpenMapper;
01751 LocalizedLaserScan* m_pScan;
01752 const List<LocalizedLaserScanList>* m_pChains;
01753 kt_bool* m_pWasChainLinked;
01754 Pose2List* m_pMeans;
01755 List<Matrix3>* m_pCovariances;
01756 kt_int32u m_MinChainSize;
01757 kt_double m_MinResponse;
01758 };
01759 #endif
01760
01761 void MapperGraph::LinkNearChains(LocalizedLaserScan* pScan, Pose2List& rMeans, List<Matrix3>& rCovariances)
01762 {
01763 const List<LocalizedLaserScanList> nearChains = FindNearChains(pScan);
01764
01765 kt_bool gotTbb = false;
01766 if (m_pOpenMapper->IsMultiThreaded())
01767 {
01768 #ifdef USE_TBB
01769 gotTbb = true;
01770 kt_bool* pWasChainLinked = new kt_bool[nearChains.Size()];
01771
01772 Pose2List means;
01773 means.Resize(nearChains.Size());
01774
01775 List<Matrix3> covariances;
01776 covariances.Resize(nearChains.Size());
01777
01778 int grainSize = 100;
01779 Parallel_LinkNearChains myTask(m_pOpenMapper, pScan, &nearChains, pWasChainLinked, &means, &covariances,
01780 m_pOpenMapper->m_pLoopMatchMinimumChainSize->GetValue(),
01781 m_pOpenMapper->m_pLinkMatchMinimumResponseFine->GetValue());
01782 tbb::parallel_for(tbb::blocked_range<kt_int32s>(0, static_cast<kt_int32s>(nearChains.Size()), grainSize), myTask);
01783
01784 for (kt_int32u i = 0; i < nearChains.Size(); i++)
01785 {
01786 if (pWasChainLinked[i] == true)
01787 {
01788 rMeans.Add(means[i]);
01789 rCovariances.Add(covariances[i]);
01790 LinkChainToScan(nearChains[i], pScan, means[i], covariances[i]);
01791 }
01792 }
01793
01794 delete [] pWasChainLinked;
01795 #endif
01796 }
01797
01798 if (gotTbb == false)
01799 {
01800 karto_const_forEach(List<LocalizedLaserScanList>, &nearChains)
01801 {
01802 #ifdef KARTO_DEBUG2
01803 std::cout << "Near chain for " << pScan->GetStateId() << ": [ ";
01804 karto_const_forEachAs(LocalizedLaserScanList, iter, iter2)
01805 {
01806 std::cout << (*iter2)->GetStateId() << " ";
01807 }
01808 std::cout << "]: ";
01809 #endif
01810
01811 if (iter->Size() < m_pOpenMapper->m_pLoopMatchMinimumChainSize->GetValue())
01812 {
01813 #ifdef KARTO_DEBUG2
01814 std::cout << iter->Size() << "(< " << m_pOpenMapper->m_pLoopMatchMinimumChainSize->GetValue() << ") REJECTED" << std::endl;
01815 #endif
01816 continue;
01817 }
01818
01819 Pose2 mean;
01820 Matrix3 covariance;
01821
01822 kt_double response = m_pOpenMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false);
01823 if (response > m_pOpenMapper->m_pLinkMatchMinimumResponseFine->GetValue() - KT_TOLERANCE)
01824 {
01825 #ifdef KARTO_DEBUG2
01826 std::cout << " ACCEPTED" << std::endl;
01827 #endif
01828 rMeans.Add(mean);
01829 rCovariances.Add(covariance);
01830 LinkChainToScan(*iter, pScan, mean, covariance);
01831 }
01832 else
01833 {
01834 #ifdef KARTO_DEBUG2
01835 std::cout << response << "(< " << m_pOpenMapper->m_pLinkMatchMinimumResponseFine->GetValue() << ") REJECTED" << std::endl;
01836 #endif
01837 }
01838 }
01839 }
01840 }
01841
01842 void MapperGraph::LinkChainToScan(const LocalizedLaserScanList& rChain, LocalizedLaserScan* pScan, const Pose2& rMean, const Matrix3& rCovariance)
01843 {
01844 Pose2 pose = pScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01845
01846 LocalizedLaserScan* pClosestScan = GetClosestScanToPose(rChain, pose);
01847 assert(pClosestScan != NULL);
01848
01849 Pose2 closestScanPose = pClosestScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01850
01851 kt_double squaredDistance = pose.GetPosition().SquaredDistance(closestScanPose.GetPosition());
01852 if (squaredDistance < math::Square(m_pOpenMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
01853 {
01854 LinkObjects(pClosestScan, pScan, rMean, rCovariance);
01855 #ifdef KARTO_DEBUG2
01856 std::cout << "Linking scan " << pScan->GetStateId() << " to chain scan " << pClosestScan->GetStateId() << std::endl;
01857 #endif
01858 }
01859 }
01860
01861 List<LocalizedLaserScanList> MapperGraph::FindNearChains(LocalizedLaserScan* pScan)
01862 {
01863 List<LocalizedLaserScanList> nearChains;
01864
01865 Pose2 scanPose = pScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01866
01867
01868 LocalizedLaserScanList processed;
01869
01870 const LocalizedLaserScanList nearLinkedScans = FindNearLinkedScans(pScan, m_pOpenMapper->m_pLinkScanMaximumDistance->GetValue());
01871 karto_const_forEach(LocalizedLaserScanList, &nearLinkedScans)
01872 {
01873 LocalizedLaserScan* pNearScan = *iter;
01874
01875 if (pNearScan == pScan)
01876 {
01877 continue;
01878 }
01879
01880
01881 if (processed.Contains(pNearScan) == true)
01882 {
01883 continue;
01884 }
01885
01886 #ifdef KARTO_DEBUG2
01887 std::cout << "BUILDING CHAIN: Scan " << pScan->GetStateId() << " is near " << pNearScan->GetStateId() << " (< " << m_pOpenMapper->m_pLinkScanMaximumDistance->GetValue() << ")" << std::endl;
01888 #endif
01889
01890 processed.Add(pNearScan);
01891
01892
01893 kt_bool isValidChain = true;
01894 LocalizedLaserScanList chain;
01895
01896 LocalizedLaserScanList scans = m_pOpenMapper->m_pMapperSensorManager->GetScans(pNearScan->GetSensorIdentifier());
01897
01898 kt_int32s nearScanIndex = m_pOpenMapper->m_pMapperSensorManager->GetScanIndex(pNearScan);
01899 assert(nearScanIndex >= 0);
01900
01901
01902 for (kt_int32s candidateScanIndex = nearScanIndex - 1; candidateScanIndex >= 0; candidateScanIndex--)
01903 {
01904 LocalizedLaserScan* pCandidateScan = scans[candidateScanIndex];
01905
01906
01907 if (pCandidateScan == pScan)
01908 {
01909 #ifdef KARTO_DEBUG2
01910 std::cout << "INVALID CHAIN: Scan " << pScan->GetStateId() << " is not allowed in chain." << std::endl;
01911 #endif
01912 isValidChain = false;
01913 }
01914
01915 Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01916 kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
01917
01918 if (squaredDistance < math::Square(m_pOpenMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
01919 {
01920 chain.Add(pCandidateScan);
01921 processed.Add(pCandidateScan);
01922
01923 #ifdef KARTO_DEBUG2
01924 std::cout << "Building chain for " << pScan->GetStateId() << ": [ ";
01925 karto_const_forEachAs(LocalizedLaserScanList, &chain, iter2)
01926 {
01927 std::cout << (*iter2)->GetStateId() << " ";
01928 }
01929 std::cout << "]" << std::endl;
01930 #endif
01931 }
01932 else
01933 {
01934 break;
01935 }
01936 }
01937
01938 chain.Add(pNearScan);
01939
01940
01941 kt_size_t end = scans.Size();
01942 for (kt_size_t candidateScanIndex = nearScanIndex + 1; candidateScanIndex < end; candidateScanIndex++)
01943 {
01944 LocalizedLaserScan* pCandidateScan = scans[candidateScanIndex];
01945
01946 if (pCandidateScan == pScan)
01947 {
01948 #ifdef KARTO_DEBUG2
01949 std::cout << "INVALID CHAIN: Scan " << pScan->GetStateId() << " is not allowed in chain." << std::endl;
01950 #endif
01951 isValidChain = false;
01952 }
01953
01954 Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());;
01955 kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
01956
01957 if (squaredDistance < math::Square(m_pOpenMapper->m_pLinkScanMaximumDistance->GetValue()) + KT_TOLERANCE)
01958 {
01959 chain.Add(pCandidateScan);
01960 processed.Add(pCandidateScan);
01961
01962 #ifdef KARTO_DEBUG2
01963 std::cout << "Building chain for " << pScan->GetStateId() << ": [ ";
01964 karto_const_forEachAs(LocalizedLaserScanList, &chain, iter2)
01965 {
01966 std::cout << (*iter2)->GetStateId() << " ";
01967 }
01968 std::cout << "]" << std::endl;
01969 #endif
01970 }
01971 else
01972 {
01973 break;
01974 }
01975 }
01976
01977 if (isValidChain)
01978 {
01979
01980 nearChains.Add(chain);
01981 }
01982 }
01983
01984 return nearChains;
01985 }
01986
01987 LocalizedLaserScanList MapperGraph::FindNearLinkedScans(LocalizedLaserScan* pScan, kt_double maxDistance)
01988 {
01989 NearScanVisitor* pVisitor = new NearScanVisitor(pScan, maxDistance, m_pOpenMapper->m_pUseScanBarycenter->GetValue());
01990 LocalizedObjectList nearLinkedObjects = m_pTraversal->Traverse(GetVertex(pScan), pVisitor);
01991 delete pVisitor;
01992
01993 LocalizedLaserScanList nearLinkedScans;
01994 karto_const_forEach(LocalizedObjectList, &nearLinkedObjects)
01995 {
01996 LocalizedObject* pObject = *iter;
01997 LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
01998 if (pScan != NULL)
01999 {
02000 nearLinkedScans.Add(pScan);
02001 }
02002 }
02003
02004 return nearLinkedScans;
02005 }
02006
02007 LocalizedLaserScanList MapperGraph::FindOverlappingScans(karto::LocalizedLaserScan *pScan)
02008 {
02009 LocalizedLaserScanList nearScans;
02010
02011 const BoundingBox2& rBoundingBox = pScan->GetBoundingBox();
02012
02013 const VertexList& vertices = GetVertices();
02014 karto_const_forEach(VertexList, &vertices)
02015 {
02016 LocalizedObject* pObject = (*iter)->GetVertexObject();
02017 LocalizedLaserScan* pCandidateScan = dynamic_cast<LocalizedLaserScan*>(pObject);
02018 if (pCandidateScan == NULL)
02019 {
02020 continue;
02021 }
02022
02023 if (rBoundingBox.Intersects(pCandidateScan->GetBoundingBox()) == true)
02024 {
02025 nearScans.Add(pCandidateScan);
02026 }
02027 }
02028
02029 return nearScans;
02030 }
02031
02032
02033 Pose2 MapperGraph::ComputeWeightedMean(const Pose2List& rMeans, const List<Matrix3>& rCovariances) const
02034 {
02035 assert(rMeans.Size() == rCovariances.Size());
02036
02037
02038 List<Matrix3> inverses;
02039 inverses.EnsureCapacity(rCovariances.Size());
02040
02041 Matrix3 sumOfInverses;
02042 karto_const_forEach(List<Matrix3>, &rCovariances)
02043 {
02044 Matrix3 inverse = iter->Inverse();
02045 inverses.Add(inverse);
02046
02047 sumOfInverses += inverse;
02048 }
02049 Matrix3 inverseOfSumOfInverses = sumOfInverses.Inverse();
02050
02051
02052 Pose2 accumulatedPose;
02053 kt_double thetaX = 0.0;
02054 kt_double thetaY = 0.0;
02055
02056 Pose2List::ConstIterator meansIter = rMeans.GetConstIterator();
02057 karto_const_forEach(List<Matrix3>, &inverses)
02058 {
02059 Pose2 pose = *meansIter;
02060 kt_double angle = pose.GetHeading();
02061 thetaX += cos(angle);
02062 thetaY += sin(angle);
02063
02064 Matrix3 weight = inverseOfSumOfInverses * (*iter);
02065 accumulatedPose += weight * pose;
02066
02067 meansIter++;
02068 }
02069
02070 thetaX /= rMeans.Size();
02071 thetaY /= rMeans.Size();
02072 accumulatedPose.SetHeading(atan2(thetaY, thetaX));
02073
02074 return accumulatedPose;
02075 }
02076
02077 LocalizedLaserScanList MapperGraph::FindPossibleLoopClosure(LocalizedLaserScan* pScan, const Identifier& rSensorName, kt_int32u& rStartScanIndex)
02078 {
02079 LocalizedLaserScanList chain;
02080
02081 Pose2 pose = pScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
02082
02083
02084
02085 const LocalizedLaserScanList nearLinkedScans = FindNearLinkedScans(pScan, m_pOpenMapper->m_pLoopSearchMaximumDistance->GetValue());
02086
02087 LocalizedLaserScanList scans = m_pOpenMapper->m_pMapperSensorManager->GetScans(rSensorName);
02088 kt_size_t nScans = scans.Size();
02089 for (; rStartScanIndex < nScans; rStartScanIndex++)
02090 {
02091 LocalizedLaserScan* pCandidateScan = scans[rStartScanIndex];
02092
02093 Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pOpenMapper->m_pUseScanBarycenter->GetValue());
02094
02095 kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition());
02096 if (squaredDistance < math::Square(m_pOpenMapper->m_pLoopSearchMaximumDistance->GetValue()) + KT_TOLERANCE)
02097 {
02098
02099 if (nearLinkedScans.Contains(pCandidateScan) == true)
02100 {
02101 chain.Clear();
02102 }
02103 else
02104 {
02105 chain.Add(pCandidateScan);
02106 }
02107 }
02108 else
02109 {
02110
02111 if (chain.Size() >= m_pOpenMapper->m_pLoopMatchMinimumChainSize->GetValue())
02112 {
02113 return chain;
02114 }
02115 else
02116 {
02117 chain.Clear();
02118 }
02119 }
02120 }
02121
02122 return chain;
02123 }
02124
02125 void MapperGraph::CorrectPoses()
02126 {
02127
02128 ScanSolver* pSolver = m_pOpenMapper->m_pScanSolver;
02129 if (pSolver != NULL)
02130 {
02131 pSolver->Compute();
02132
02133 karto_const_forEach(ScanSolver::IdPoseVector, &pSolver->GetCorrections())
02134 {
02135 LocalizedObject* pObject = m_pOpenMapper->m_pMapperSensorManager->GetLocalizedObject(iter->GetFirst());
02136 LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
02137
02138 if (pScan != NULL)
02139 {
02140 pScan->SetSensorPose(iter->GetSecond());
02141 }
02142 else
02143 {
02144 pObject->SetCorrectedPose(iter->GetSecond());
02145 }
02146 }
02147
02148 pSolver->Clear();
02149 }
02150 }
02151
02155
02159 OpenMapper::OpenMapper(kt_bool multiThreaded)
02160 : Module("OpenMapper")
02161 , m_pScanSolver(NULL)
02162 , m_Initialized(false)
02163 , m_MultiThreaded(multiThreaded)
02164 , m_pSequentialScanMatcher(NULL)
02165 , m_pMapperSensorManager(NULL)
02166 , m_pGraph(NULL)
02167 {
02168 InitializeParameters();
02169 }
02170
02174 OpenMapper::OpenMapper(const char* pName, kt_bool multiThreaded)
02175 : Module(pName)
02176 , m_pScanSolver(NULL)
02177 , m_Initialized(false)
02178 , m_MultiThreaded(multiThreaded)
02179 , m_pSequentialScanMatcher(NULL)
02180 , m_pMapperSensorManager(NULL)
02181 , m_pGraph(NULL)
02182 {
02183 InitializeParameters();
02184 }
02185
02189 OpenMapper::~OpenMapper()
02190 {
02191 Reset();
02192
02193 delete m_pMapperSensorManager;
02194 }
02195
02196 void OpenMapper::InitializeParameters()
02197 {
02198 m_pUseScanMatching = new Parameter<kt_bool>(GetParameterSet(), "UseScanMatching", "Mapper::Use::Scan Matching", "UseScanMatching", true);
02199 m_pUseScanBarycenter = new Parameter<kt_bool>(GetParameterSet(), "UseScanBarycenter", "Mapper::Use::Scan Barycenter", "UseScanBarycenter", true);
02200 m_pMinimumTravelDistance = new Parameter<kt_double>(GetParameterSet(), "MinimumTravelDistance", "Mapper::Minimum Travel::Distance", "MinimumTravelDistance", 0.2);
02201 m_pMinimumTravelHeading = new Parameter<kt_double>(GetParameterSet(), "MinimumTravelHeading", "Mapper::Minimum Travel::Heading", "MinimumTravelHeading", math::DegreesToRadians(20));
02202
02203 m_pScanBufferSize = new Parameter<kt_int32u>(GetParameterSet(), "ScanBufferSize", "Mapper::Scan Buffer::Size", "ScanBufferSize", 70);
02204 m_pScanBufferMaximumScanDistance = new Parameter<kt_double>(GetParameterSet(), "ScanBufferMaximumScanDistance", "Mapper::Scan Buffer::Maximum Scan Distance", "ScanBufferMaximumScanDistance", 20);
02205 m_pUseResponseExpansion = new Parameter<kt_bool>(GetParameterSet(), "UseResponseExpansion", "Mapper::Use::Response Expansion", "UseResponseExpansion", false);
02206
02207 m_pDistanceVariancePenalty = new Parameter<kt_double>(GetParameterSet(), "DistanceVariancePenalty", "Mapper::Scan Matcher::Distance Variance Penalty", "DistanceVariancePenalty", math::Square(0.3));
02208 m_pMinimumDistancePenalty = new Parameter<kt_double>(GetParameterSet(), "MinimumDistancePenalty", "Mapper::Scan Matcher::Minimum Distance Penalty", "MinimumDistancePenalty", 0.5);
02209 m_pAngleVariancePenalty = new Parameter<kt_double>(GetParameterSet(), "AngleVariancePenalty", "Mapper::Scan Matcher::Angle Variance Penalty", "AngleVariancePenalty", math::Square(math::DegreesToRadians(20)));
02210 m_pMinimumAnglePenalty = new Parameter<kt_double>(GetParameterSet(), "MinimumAnglePenalty", "Mapper::Scan Matcher::Minimum Angle Penalty", "MinimumAnglePenalty", 0.9);
02211
02212 m_pLinkMatchMinimumResponseFine = new Parameter<kt_double>(GetParameterSet(), "LinkMatchMinimumResponseFine", "Mapper::Link::Match Minimum Response Fine", "LinkMatchMinimumResponseFine", 0.6);
02213 m_pLinkScanMaximumDistance = new Parameter<kt_double>(GetParameterSet(), "LinkScanMaximumDistance", "Mapper::Link::Scan Maximum Distance", "LinkScanMaximumDistance", 5.0);
02214
02215 m_pCorrelationSearchSpaceDimension = new Parameter<kt_double>(GetParameterSet(), "CorrelationSearchSpaceDimension", "Mapper::Correlation Search Space::Dimension", "CorrelationSearchSpaceDimension", 0.3);
02216 m_pCorrelationSearchSpaceResolution = new Parameter<kt_double>(GetParameterSet(), "CorrelationSearchSpaceResolution", "Mapper::Correlation Search Space::Resolution", "CorrelationSearchSpaceResolution", 0.01);
02217 m_pCorrelationSearchSpaceSmearDeviation = new Parameter<kt_double>(GetParameterSet(), "CorrelationSearchSpaceSmearDeviation", "Mapper::Correlation Search Space::Smear Deviation", "CorrelationSearchSpaceSmearDeviation", 0.03);
02218 m_pCoarseSearchAngleOffset = new Parameter<kt_double>(GetParameterSet(), "CoarseSearchAngleOffset", "Mapper::Scan Matcher::Coarse Search Angle Offset", "CoarseSearchAngleOffset", math::DegreesToRadians(20));
02219 m_pFineSearchAngleOffset = new Parameter<kt_double>(GetParameterSet(), "FineSearchAngleOffset", "Mapper::Scan Matcher::Fine Search Angle Offset", "FineSearchAngleOffset", math::DegreesToRadians(0.2));
02220 m_pCoarseAngleResolution = new Parameter<kt_double>(GetParameterSet(), "CoarseAngleResolution", "Mapper::Scan Matcher::Coarse Angle Resolution", "CoarseAngleResolution", math::DegreesToRadians(2));
02221
02222 m_pLoopSearchSpaceDimension = new Parameter<kt_double>(GetParameterSet(), "LoopSearchSpaceDimension", "Mapper::Loop Correlation Search Space::Dimension", "LoopSearchSpaceDimension", 8.0);
02223 m_pLoopSearchSpaceResolution = new Parameter<kt_double>(GetParameterSet(), "LoopSearchSpaceResolution", "Mapper::Loop Correlation Search Space::Resolution", "LoopSearchSpaceResolution", 0.05);
02224 m_pLoopSearchSpaceSmearDeviation = new Parameter<kt_double>(GetParameterSet(), "LoopSearchSpaceSmearDeviation", "Mapper::Loop Correlation Search Space::Smear Deviation", "LoopSearchSpaceSmearDeviation", 0.03);
02225
02226 m_pLoopSearchMaximumDistance = new Parameter<kt_double>(GetParameterSet(), "LoopSearchMaximumDistance", "Mapper::Loop::Search Maximum Distance", "LoopSearchMaximumDistance", 4.0);
02227 m_pLoopMatchMinimumChainSize = new Parameter<kt_int32u>(GetParameterSet(), "LoopMatchMinimumChainSize", "Mapper::Loop::Match::Minimum Chain Size", "LoopMatchMinimumChainSize", 10);
02228 m_pLoopMatchMaximumVarianceCoarse = new Parameter<kt_double>(GetParameterSet(), "LoopMatchMaximumVarianceCoarse", "Mapper::Loop::Match::Maximum Variance Coarse", "LoopMatchMaximumVarianceCoarse", math::Square(0.4));
02229 m_pLoopMatchMinimumResponseCoarse = new Parameter<kt_double>(GetParameterSet(), "LoopMatchMinimumResponseCoarse", "Mapper::Loop::Match::Minimum Response Coarse", "LoopMatchMinimumResponseCoarse", 0.7);
02230 m_pLoopMatchMinimumResponseFine = new Parameter<kt_double>(GetParameterSet(), "LoopMatchMinimumResponseFine", "Mapper::Loop::Match::Minimum Response Fine", "LoopMatchMinimumResponseFine", 0.7);
02231 }
02232
02233 void OpenMapper::Initialize(kt_double rangeThreshold)
02234 {
02235 if (m_Initialized == false)
02236 {
02237
02238 m_pSequentialScanMatcher = ScanMatcher::Create(this, m_pCorrelationSearchSpaceDimension->GetValue(), m_pCorrelationSearchSpaceResolution->GetValue(), m_pCorrelationSearchSpaceSmearDeviation->GetValue(), rangeThreshold);
02239 assert(m_pSequentialScanMatcher);
02240
02241 m_pMapperSensorManager = new MapperSensorManager(m_pScanBufferSize->GetValue(), m_pScanBufferMaximumScanDistance->GetValue());
02242
02243 m_pGraph = new MapperGraph(this, rangeThreshold);
02244
02245 m_Initialized = true;
02246 }
02247 else
02248 {
02249 Log(LOG_WARNING, "Mapper already initialized");
02250 }
02251 }
02252
02253 void OpenMapper::Reset()
02254 {
02255 Module::Reset();
02256
02257 delete m_pSequentialScanMatcher;
02258 m_pSequentialScanMatcher = NULL;
02259
02260 delete m_pGraph;
02261 m_pGraph = NULL;
02262
02263 delete m_pMapperSensorManager;
02264 m_pMapperSensorManager = NULL;
02265
02266 m_Initialized = false;
02267 }
02268
02269 kt_bool OpenMapper::Process(Object* pObject)
02270 {
02271 if (pObject == NULL)
02272 {
02273 return false;
02274 }
02275
02276 kt_bool isObjectProcessed = Module::Process(pObject);
02277
02278 if (IsLaserRangeFinder(pObject))
02279 {
02280 LaserRangeFinder* pLaserRangeFinder = dynamic_cast<LaserRangeFinder*>(pObject);
02281
02282 if (m_Initialized == false)
02283 {
02284
02285 Initialize(pLaserRangeFinder->GetRangeThreshold());
02286 }
02287
02288
02289 m_pMapperSensorManager->RegisterSensor(pLaserRangeFinder->GetIdentifier());
02290
02291 return true;
02292 }
02293
02294 LocalizedObject* pLocalizedObject = dynamic_cast<LocalizedObject*>(pObject);
02295 if (pLocalizedObject != NULL)
02296 {
02297 LocalizedLaserScan* pScan = dynamic_cast<LocalizedLaserScan*>(pObject);
02298 if (pScan != NULL)
02299 {
02300 karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();
02301
02302
02303 if (pLaserRangeFinder == NULL)
02304 {
02305 return false;
02306 }
02307
02308
02309 pLaserRangeFinder->Validate(pScan);
02310
02311 if (m_Initialized == false)
02312 {
02313
02314 Initialize(pLaserRangeFinder->GetRangeThreshold());
02315 }
02316 }
02317
02318
02319 m_pMapperSensorManager->RegisterSensor(pLocalizedObject->GetSensorIdentifier());
02320
02321
02322 LocalizedLaserScan* pLastScan = m_pMapperSensorManager->GetLastScan(pLocalizedObject->GetSensorIdentifier());
02323
02324
02325 if (pLastScan != NULL)
02326 {
02327 Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose());
02328 pLocalizedObject->SetCorrectedPose(lastTransform.TransformPose(pLocalizedObject->GetOdometricPose()));
02329 }
02330
02331
02332
02333 if (pScan == NULL || (!HasMovedEnough(pScan, pLastScan) && !pScan->IsGpsReadingValid()))
02334 {
02335 if (pLocalizedObject->HasCustomItem() == true)
02336 {
02337 m_pMapperSensorManager->AddLocalizedObject(pLocalizedObject);
02338
02339
02340 m_pGraph->AddVertex(pLocalizedObject);
02341 m_pGraph->AddEdges(pLocalizedObject);
02342
02343 return true;
02344 }
02345
02346 return false;
02347 }
02348
02350
02351
02352 Matrix3 covariance;
02353 covariance.SetToIdentity();
02354
02355
02356 if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
02357 {
02358 Pose2 bestPose;
02359 m_pSequentialScanMatcher->MatchScan(pScan, m_pMapperSensorManager->GetRunningScans(pScan->GetSensorIdentifier()), bestPose, covariance);
02360 pScan->SetSensorPose(bestPose);
02361 }
02362
02363 ScanMatched(pScan);
02364
02365
02366 m_pMapperSensorManager->AddLocalizedObject(pLocalizedObject);
02367
02368 if (m_pUseScanMatching->GetValue())
02369 {
02370
02371 m_pGraph->AddVertex(pScan);
02372 m_pGraph->AddEdges(pScan, covariance);
02373
02374 m_pMapperSensorManager->AddRunningScan(pScan);
02375
02376 List<Identifier> sensorNames = m_pMapperSensorManager->GetSensorNames();
02377 karto_const_forEach(List<Identifier>, &sensorNames)
02378 {
02379 m_pGraph->TryCloseLoop(pScan, *iter);
02380 }
02381 }
02382
02383 m_pMapperSensorManager->SetLastScan(pScan);
02384
02385 ScanMatchingEnd(pScan);
02386
02387 isObjectProcessed = true;
02388 }
02389
02390 return isObjectProcessed;
02391 }
02392
02393 kt_bool OpenMapper::HasMovedEnough(LocalizedLaserScan* pScan, LocalizedLaserScan* pLastScan) const
02394 {
02395
02396 if (pLastScan == NULL)
02397 {
02398 return true;
02399 }
02400
02401 Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose());
02402 Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose());
02403
02404
02405 kt_double deltaHeading = math::NormalizeAngle(scannerPose.GetHeading() - lastScannerPose.GetHeading());
02406 if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue())
02407 {
02408 return true;
02409 }
02410
02411
02412 kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition());
02413 if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE)
02414 {
02415 return true;
02416 }
02417
02418 return false;
02419 }
02420
02421 const LocalizedLaserScanList OpenMapper::GetAllProcessedScans() const
02422 {
02423 LocalizedLaserScanList allScans;
02424
02425 if (m_pMapperSensorManager != NULL)
02426 {
02427 allScans = m_pMapperSensorManager->GetAllScans();
02428 }
02429
02430 return allScans;
02431 }
02432
02433 const LocalizedObjectList OpenMapper::GetAllProcessedObjects() const
02434 {
02435 LocalizedObjectList allObjects;
02436
02437 if (m_pMapperSensorManager != NULL)
02438 {
02439
02440 allObjects = m_pMapperSensorManager->GetAllObjects();
02441 }
02442
02443 return allObjects;
02444 }
02445
02446 ScanSolver* OpenMapper::GetScanSolver() const
02447 {
02448 return m_pScanSolver;
02449 }
02450
02451 void OpenMapper::SetScanSolver(ScanSolver* pScanOptimizer)
02452 {
02453 m_pScanSolver = pScanOptimizer;
02454 }
02455
02456 MapperGraph* OpenMapper::GetGraph() const
02457 {
02458 return m_pGraph;
02459 }
02460
02461 ScanMatcher* OpenMapper::GetSequentialScanMatcher() const
02462 {
02463 return m_pSequentialScanMatcher;
02464 }
02465
02466 ScanMatcher* OpenMapper::GetLoopScanMatcher() const
02467 {
02468 return m_pGraph->GetLoopScanMatcher();
02469 }
02470
02471 }