Mapper.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010 SRI International
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include <sstream>
19 #include <fstream>
20 #include <stdexcept>
21 #include <queue>
22 #include <set>
23 #include <list>
24 #include <iterator>
25 
26 #include <math.h>
27 #include <assert.h>
28 
29 #include "open_karto/Mapper.h"
30 
31 namespace karto
32 {
33 
34  // enable this for verbose debug information
35  // #define KARTO_DEBUG
36 
37  #define MAX_VARIANCE 500.0
38  #define DISTANCE_PENALTY_GAIN 0.2
39  #define ANGLE_PENALTY_GAIN 0.2
40 
44 
49  {
50  public:
54  ScanManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
55  : m_pLastScan(NULL)
56  , m_RunningBufferMaximumSize(runningBufferMaximumSize)
57  , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
58  {
59  }
60 
64  virtual ~ScanManager()
65  {
66  Clear();
67  }
68 
69  public:
74  inline void AddScan(LocalizedRangeScan* pScan, kt_int32s uniqueId)
75  {
76  // assign state id to scan
77  pScan->SetStateId(static_cast<kt_int32u>(m_Scans.size()));
78 
79  // assign unique id to scan
80  pScan->SetUniqueId(uniqueId);
81 
82  // add it to scan buffer
83  m_Scans.push_back(pScan);
84  }
85 
92  {
93  return m_pLastScan;
94  }
95 
100  inline void SetLastScan(LocalizedRangeScan* pScan)
101  {
102  m_pLastScan = pScan;
103  }
104 
110  {
111  return m_Scans;
112  }
113 
119  {
120  return m_RunningScans;
121  }
122 
128  {
129  m_RunningScans.push_back(pScan);
130 
131  // vector has at least one element (first line of this function), so this is valid
132  Pose2 frontScanPose = m_RunningScans.front()->GetSensorPose();
133  Pose2 backScanPose = m_RunningScans.back()->GetSensorPose();
134 
135  // cap vector size and remove all scans from front of vector that are too far from end of vector
136  kt_double squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
137  while (m_RunningScans.size() > m_RunningBufferMaximumSize ||
139  {
140  // remove front of running scans
141  m_RunningScans.erase(m_RunningScans.begin());
142 
143  // recompute stats of running scans
144  frontScanPose = m_RunningScans.front()->GetSensorPose();
145  backScanPose = m_RunningScans.back()->GetSensorPose();
146  squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
147  }
148  }
149 
153  void Clear()
154  {
155  m_Scans.clear();
156  m_RunningScans.clear();
157  }
158 
159  private:
163 
166  }; // ScanManager
167 
171 
172  void MapperSensorManager::RegisterSensor(const Name& rSensorName)
173  {
174  if (GetScanManager(rSensorName) == NULL)
175  {
176  m_ScanManagers[rSensorName] = new ScanManager(m_RunningBufferMaximumSize, m_RunningBufferMaximumDistance);
177  }
178  }
179 
180 
188  {
189  ScanManager* pScanManager = GetScanManager(rSensorName);
190  if (pScanManager != NULL)
191  {
192  return pScanManager->GetScans().at(scanIndex);
193  }
194 
195  assert(false);
196  return NULL;
197  }
198 
205  {
206  RegisterSensor(rSensorName);
207 
208  return GetScanManager(rSensorName)->GetLastScan();
209  }
210 
216  {
217  GetScanManager(pScan)->SetLastScan(pScan);
218  }
219 
225  {
226  GetScanManager(pScan)->AddScan(pScan, m_NextScanId);
227  m_Scans.push_back(pScan);
228  m_NextScanId++;
229  }
230 
236  {
237  GetScanManager(pScan)->AddRunningScan(pScan);
238  }
239 
246  {
247  return GetScanManager(rSensorName)->GetScans();
248  }
249 
256  {
257  return GetScanManager(rSensorName)->GetRunningScans();
258  }
259 
265  {
267 
268  forEach(ScanManagerMap, &m_ScanManagers)
269  {
270  LocalizedRangeScanVector& rScans = iter->second->GetScans();
271 
272  scans.insert(scans.end(), rScans.begin(), rScans.end());
273  }
274 
275  return scans;
276  }
277 
282  {
283 // SensorManager::Clear();
284 
285  forEach(ScanManagerMap, &m_ScanManagers)
286  {
287  delete iter->second;
288  }
289 
290  m_ScanManagers.clear();
291  }
292 
296 
298  {
299  delete m_pCorrelationGrid;
300  delete m_pSearchSpaceProbs;
301  delete m_pGridLookup;
302  }
303 
304  ScanMatcher* ScanMatcher::Create(Mapper* pMapper, kt_double searchSize, kt_double resolution,
305  kt_double smearDeviation, kt_double rangeThreshold)
306  {
307  // invalid parameters
308  if (resolution <= 0)
309  {
310  return NULL;
311  }
312  if (searchSize <= 0)
313  {
314  return NULL;
315  }
316  if (smearDeviation < 0)
317  {
318  return NULL;
319  }
320  if (rangeThreshold <= 0)
321  {
322  return NULL;
323  }
324 
325  assert(math::DoubleEqual(math::Round(searchSize / resolution), (searchSize / resolution)));
326 
327  // calculate search space in grid coordinates
328  kt_int32u searchSpaceSideSize = static_cast<kt_int32u>(math::Round(searchSize / resolution) + 1);
329 
330  // compute requisite size of correlation grid (pad grid so that scan points can't fall off the grid
331  // if a scan is on the border of the search space)
332  kt_int32u pointReadingMargin = static_cast<kt_int32u>(ceil(rangeThreshold / resolution));
333 
334  kt_int32s gridSize = searchSpaceSideSize + 2 * pointReadingMargin;
335 
336  // create correlation grid
337  assert(gridSize % 2 == 1);
338  CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation);
339 
340  // create search space probabilities
341  Grid<kt_double>* pSearchSpaceProbs = Grid<kt_double>::CreateGrid(searchSpaceSideSize,
342  searchSpaceSideSize, resolution);
343 
344  ScanMatcher* pScanMatcher = new ScanMatcher(pMapper);
345  pScanMatcher->m_pCorrelationGrid = pCorrelationGrid;
346  pScanMatcher->m_pSearchSpaceProbs = pSearchSpaceProbs;
347  pScanMatcher->m_pGridLookup = new GridIndexLookup<kt_int8u>(pCorrelationGrid);
348 
349  return pScanMatcher;
350  }
351 
363  Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch)
364  {
366  // set scan pose to be center of grid
367 
368  // 1. get scan position
369  Pose2 scanPose = pScan->GetSensorPose();
370 
371  // scan has no readings; cannot do scan matching
372  // best guess of pose is based off of adjusted odometer reading
373  if (pScan->GetNumberOfRangeReadings() == 0)
374  {
375  rMean = scanPose;
376 
377  // maximum covariance
378  rCovariance(0, 0) = MAX_VARIANCE; // XX
379  rCovariance(1, 1) = MAX_VARIANCE; // YY
380  rCovariance(2, 2) = 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue()); // TH*TH
381 
382  return 0.0;
383  }
384 
385  // 2. get size of grid
386  Rectangle2<kt_int32s> roi = m_pCorrelationGrid->GetROI();
387 
388  // 3. compute offset (in meters - lower left corner)
389  Vector2<kt_double> offset;
390  offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * m_pCorrelationGrid->GetResolution()));
391  offset.SetY(scanPose.GetY() - (0.5 * (roi.GetHeight() - 1) * m_pCorrelationGrid->GetResolution()));
392 
393  // 4. set offset
394  m_pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset);
395 
397 
398  // set up correlation grid
399  AddScans(rBaseScans, scanPose.GetPosition());
400 
401  // compute how far to search in each direction
402  Vector2<kt_double> searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight());
403  Vector2<kt_double> coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(),
404  0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution());
405 
406  // a coarse search only checks half the cells in each dimension
407  Vector2<kt_double> coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(),
408  2 * m_pCorrelationGrid->GetResolution());
409 
410  // actual scan-matching
411  kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
412  m_pMapper->m_pCoarseSearchAngleOffset->GetValue(),
413  m_pMapper->m_pCoarseAngleResolution->GetValue(),
414  doPenalize, rMean, rCovariance, false);
415 
416  if (m_pMapper->m_pUseResponseExpansion->GetValue() == true)
417  {
418  if (math::DoubleEqual(bestResponse, 0.0))
419  {
420 #ifdef KARTO_DEBUG
421  std::cout << "Mapper Info: Expanding response search space!" << std::endl;
422 #endif
423  // try and increase search angle offset with 20 degrees and do another match
424  kt_double newSearchAngleOffset = m_pMapper->m_pCoarseSearchAngleOffset->GetValue();
425  for (kt_int32u i = 0; i < 3; i++)
426  {
427  newSearchAngleOffset += math::DegreesToRadians(20);
428 
429  bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
430  newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(),
431  doPenalize, rMean, rCovariance, false);
432 
433  if (math::DoubleEqual(bestResponse, 0.0) == false)
434  {
435  break;
436  }
437  }
438 
439 #ifdef KARTO_DEBUG
440  if (math::DoubleEqual(bestResponse, 0.0))
441  {
442  std::cout << "Mapper Warning: Unable to calculate response!" << std::endl;
443  }
444 #endif
445  }
446  }
447 
448  if (doRefineMatch)
449  {
450  Vector2<kt_double> fineSearchOffset(coarseSearchResolution * 0.5);
451  Vector2<kt_double> fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution());
452  bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution,
453  0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(),
454  m_pMapper->m_pFineSearchAngleOffset->GetValue(),
455  doPenalize, rMean, rCovariance, true);
456  }
457 
458 #ifdef KARTO_DEBUG
459  std::cout << " BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ", VARIANCE = "
460  << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl;
461 #endif
462  assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
463 
464  return bestResponse;
465  }
466 
484  const Vector2<kt_double>& rSearchSpaceOffset,
485  const Vector2<kt_double>& rSearchSpaceResolution,
486  kt_double searchAngleOffset, kt_double searchAngleResolution,
487  kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch)
488  {
489  assert(searchAngleResolution != 0.0);
490 
491  // setup lookup arrays
492  m_pGridLookup->ComputeOffsets(pScan, rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution);
493 
494  // only initialize probability grid if computing positional covariance (during coarse match)
495  if (!doingFineMatch)
496  {
497  m_pSearchSpaceProbs->Clear();
498 
499  // position search grid - finds lower left corner of search grid
500  Vector2<kt_double> offset(rSearchCenter.GetPosition() - rSearchSpaceOffset);
501  m_pSearchSpaceProbs->GetCoordinateConverter()->SetOffset(offset);
502  }
503 
504  // calculate position arrays
505 
506  std::vector<kt_double> xPoses;
507  kt_int32u nX = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetX() *
508  2.0 / rSearchSpaceResolution.GetX()) + 1);
509  kt_double startX = -rSearchSpaceOffset.GetX();
510  for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
511  {
512  xPoses.push_back(startX + xIndex * rSearchSpaceResolution.GetX());
513  }
514  assert(math::DoubleEqual(xPoses.back(), -startX));
515 
516  std::vector<kt_double> yPoses;
517  kt_int32u nY = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetY() *
518  2.0 / rSearchSpaceResolution.GetY()) + 1);
519  kt_double startY = -rSearchSpaceOffset.GetY();
520  for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
521  {
522  yPoses.push_back(startY + yIndex * rSearchSpaceResolution.GetY());
523  }
524  assert(math::DoubleEqual(yPoses.back(), -startY));
525 
526  // calculate pose response array size
527  kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1);
528 
529  kt_int32u poseResponseSize = static_cast<kt_int32u>(xPoses.size() * yPoses.size() * nAngles);
530 
531  // allocate array
532  std::pair<kt_double, Pose2>* pPoseResponse = new std::pair<kt_double, Pose2>[poseResponseSize];
533 
534  Vector2<kt_int32s> startGridPoint = m_pCorrelationGrid->WorldToGrid(Vector2<kt_double>(rSearchCenter.GetX()
535  + startX, rSearchCenter.GetY() + startY));
536 
537  kt_int32u poseResponseCounter = 0;
538  forEachAs(std::vector<kt_double>, &yPoses, yIter)
539  {
540  kt_double y = *yIter;
541  kt_double newPositionY = rSearchCenter.GetY() + y;
542  kt_double squareY = math::Square(y);
543 
544  forEachAs(std::vector<kt_double>, &xPoses, xIter)
545  {
546  kt_double x = *xIter;
547  kt_double newPositionX = rSearchCenter.GetX() + x;
548  kt_double squareX = math::Square(x);
549 
550  Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(Vector2<kt_double>(newPositionX, newPositionY));
551  kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);
552  assert(gridIndex >= 0);
553 
554  kt_double angle = 0.0;
555  kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
556  for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
557  {
558  angle = startAngle + angleIndex * searchAngleResolution;
559 
560  kt_double response = GetResponse(angleIndex, gridIndex);
561  if (doPenalize && (math::DoubleEqual(response, 0.0) == false))
562  {
563  // simple model (approximate Gaussian) to take odometry into account
564 
565  kt_double squaredDistance = squareX + squareY;
566  kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN *
567  squaredDistance / m_pMapper->m_pDistanceVariancePenalty->GetValue());
568  distancePenalty = math::Maximum(distancePenalty, m_pMapper->m_pMinimumDistancePenalty->GetValue());
569 
570  kt_double squaredAngleDistance = math::Square(angle - rSearchCenter.GetHeading());
571  kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN *
572  squaredAngleDistance / m_pMapper->m_pAngleVariancePenalty->GetValue());
573  anglePenalty = math::Maximum(anglePenalty, m_pMapper->m_pMinimumAnglePenalty->GetValue());
574 
575  response *= (distancePenalty * anglePenalty);
576  }
577 
578  // store response and pose
579  pPoseResponse[poseResponseCounter] = std::pair<kt_double, Pose2>(response, Pose2(newPositionX, newPositionY,
580  math::NormalizeAngle(angle)));
581  poseResponseCounter++;
582  }
583 
584  assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
585  }
586  }
587 
588  assert(poseResponseSize == poseResponseCounter);
589 
590  // find value of best response (in [0; 1])
591  kt_double bestResponse = -1;
592  for (kt_int32u i = 0; i < poseResponseSize; i++)
593  {
594  bestResponse = math::Maximum(bestResponse, pPoseResponse[i].first);
595 
596  // will compute positional covariance, save best relative probability for each cell
597  if (!doingFineMatch)
598  {
599  const Pose2& rPose = pPoseResponse[i].second;
600  Vector2<kt_int32s> grid = m_pSearchSpaceProbs->WorldToGrid(rPose.GetPosition());
601 
602  // Changed (kt_double*) to the reinterpret_cast - Luc
603  kt_double* ptr = reinterpret_cast<kt_double*>(m_pSearchSpaceProbs->GetDataPointer(grid));
604  if (ptr == NULL)
605  {
606  throw std::runtime_error("Mapper FATAL ERROR - Index out of range in probability search!");
607  }
608 
609  *ptr = math::Maximum(pPoseResponse[i].first, *ptr);
610  }
611  }
612 
613  // average all poses with same highest response
614  Vector2<kt_double> averagePosition;
615  kt_double thetaX = 0.0;
616  kt_double thetaY = 0.0;
617  kt_int32s averagePoseCount = 0;
618  for (kt_int32u i = 0; i < poseResponseSize; i++)
619  {
620  if (math::DoubleEqual(pPoseResponse[i].first, bestResponse))
621  {
622  averagePosition += pPoseResponse[i].second.GetPosition();
623 
624  kt_double heading = pPoseResponse[i].second.GetHeading();
625  thetaX += cos(heading);
626  thetaY += sin(heading);
627 
628  averagePoseCount++;
629  }
630  }
631 
632  Pose2 averagePose;
633  if (averagePoseCount > 0)
634  {
635  averagePosition /= averagePoseCount;
636 
637  thetaX /= averagePoseCount;
638  thetaY /= averagePoseCount;
639 
640  averagePose = Pose2(averagePosition, atan2(thetaY, thetaX));
641  }
642  else
643  {
644  throw std::runtime_error("Mapper FATAL ERROR - Unable to find best position");
645  }
646 
647  // delete pose response array
648  delete [] pPoseResponse;
649 
650 #ifdef KARTO_DEBUG
651  std::cout << "bestPose: " << averagePose << std::endl;
652  std::cout << "bestResponse: " << bestResponse << std::endl;
653 #endif
654 
655  if (!doingFineMatch)
656  {
657  ComputePositionalCovariance(averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset,
658  rSearchSpaceResolution, searchAngleResolution, rCovariance);
659  }
660  else
661  {
662  ComputeAngularCovariance(averagePose, bestResponse, rSearchCenter,
663  searchAngleOffset, searchAngleResolution, rCovariance);
664  }
665 
666  rMean = averagePose;
667 
668 #ifdef KARTO_DEBUG
669  std::cout << "bestPose: " << averagePose << std::endl;
670 #endif
671 
672  if (bestResponse > 1.0)
673  {
674  bestResponse = 1.0;
675  }
676 
677  assert(math::InRange(bestResponse, 0.0, 1.0));
678  assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
679 
680  return bestResponse;
681  }
682 
693  void ScanMatcher::ComputePositionalCovariance(const Pose2& rBestPose, kt_double bestResponse,
694  const Pose2& rSearchCenter,
695  const Vector2<kt_double>& rSearchSpaceOffset,
696  const Vector2<kt_double>& rSearchSpaceResolution,
697  kt_double searchAngleResolution, Matrix3& rCovariance)
698  {
699  // reset covariance to identity matrix
700  rCovariance.SetToIdentity();
701 
702  // if best response is vary small return max variance
703  if (bestResponse < KT_TOLERANCE)
704  {
705  rCovariance(0, 0) = MAX_VARIANCE; // XX
706  rCovariance(1, 1) = MAX_VARIANCE; // YY
707  rCovariance(2, 2) = 4 * math::Square(searchAngleResolution); // TH*TH
708 
709  return;
710  }
711 
712  kt_double accumulatedVarianceXX = 0;
713  kt_double accumulatedVarianceXY = 0;
714  kt_double accumulatedVarianceYY = 0;
715  kt_double norm = 0;
716 
717  kt_double dx = rBestPose.GetX() - rSearchCenter.GetX();
718  kt_double dy = rBestPose.GetY() - rSearchCenter.GetY();
719 
720  kt_double offsetX = rSearchSpaceOffset.GetX();
721  kt_double offsetY = rSearchSpaceOffset.GetY();
722 
723  kt_int32u nX = static_cast<kt_int32u>(math::Round(offsetX * 2.0 / rSearchSpaceResolution.GetX()) + 1);
724  kt_double startX = -offsetX;
725  assert(math::DoubleEqual(startX + (nX - 1) * rSearchSpaceResolution.GetX(), -startX));
726 
727  kt_int32u nY = static_cast<kt_int32u>(math::Round(offsetY * 2.0 / rSearchSpaceResolution.GetY()) + 1);
728  kt_double startY = -offsetY;
729  assert(math::DoubleEqual(startY + (nY - 1) * rSearchSpaceResolution.GetY(), -startY));
730 
731  for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
732  {
733  kt_double y = startY + yIndex * rSearchSpaceResolution.GetY();
734 
735  for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
736  {
737  kt_double x = startX + xIndex * rSearchSpaceResolution.GetX();
738 
739  Vector2<kt_int32s> gridPoint = m_pSearchSpaceProbs->WorldToGrid(Vector2<kt_double>(rSearchCenter.GetX() + x,
740  rSearchCenter.GetY() + y));
741  kt_double response = *(m_pSearchSpaceProbs->GetDataPointer(gridPoint));
742 
743  // response is not a low response
744  if (response >= (bestResponse - 0.1))
745  {
746  norm += response;
747  accumulatedVarianceXX += (math::Square(x - dx) * response);
748  accumulatedVarianceXY += ((x - dx) * (y - dy) * response);
749  accumulatedVarianceYY += (math::Square(y - dy) * response);
750  }
751  }
752  }
753 
754  if (norm > KT_TOLERANCE)
755  {
756  kt_double varianceXX = accumulatedVarianceXX / norm;
757  kt_double varianceXY = accumulatedVarianceXY / norm;
758  kt_double varianceYY = accumulatedVarianceYY / norm;
759  kt_double varianceTHTH = 4 * math::Square(searchAngleResolution);
760 
761  // lower-bound variances so that they are not too small;
762  // ensures that links are not too tight
763  kt_double minVarianceXX = 0.1 * math::Square(rSearchSpaceResolution.GetX());
764  kt_double minVarianceYY = 0.1 * math::Square(rSearchSpaceResolution.GetY());
765  varianceXX = math::Maximum(varianceXX, minVarianceXX);
766  varianceYY = math::Maximum(varianceYY, minVarianceYY);
767 
768  // increase variance for poorer responses
769  kt_double multiplier = 1.0 / bestResponse;
770  rCovariance(0, 0) = varianceXX * multiplier;
771  rCovariance(0, 1) = varianceXY * multiplier;
772  rCovariance(1, 0) = varianceXY * multiplier;
773  rCovariance(1, 1) = varianceYY * multiplier;
774  rCovariance(2, 2) = varianceTHTH; // this value will be set in ComputeAngularCovariance
775  }
776 
777  // if values are 0, set to MAX_VARIANCE
778  // values might be 0 if points are too sparse and thus don't hit other points
779  if (math::DoubleEqual(rCovariance(0, 0), 0.0))
780  {
781  rCovariance(0, 0) = MAX_VARIANCE;
782  }
783 
784  if (math::DoubleEqual(rCovariance(1, 1), 0.0))
785  {
786  rCovariance(1, 1) = MAX_VARIANCE;
787  }
788  }
789 
800  kt_double bestResponse,
801  const Pose2& rSearchCenter,
802  kt_double searchAngleOffset,
803  kt_double searchAngleResolution,
804  Matrix3& rCovariance)
805  {
806  // NOTE: do not reset covariance matrix
807 
808  // normalize angle difference
809  kt_double bestAngle = math::NormalizeAngleDifference(rBestPose.GetHeading(), rSearchCenter.GetHeading());
810 
811  Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(rBestPose.GetPosition());
812  kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);
813 
814  kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2 / searchAngleResolution) + 1);
815 
816  kt_double angle = 0.0;
817  kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
818 
819  kt_double norm = 0.0;
820  kt_double accumulatedVarianceThTh = 0.0;
821  for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
822  {
823  angle = startAngle + angleIndex * searchAngleResolution;
824  kt_double response = GetResponse(angleIndex, gridIndex);
825 
826  // response is not a low response
827  if (response >= (bestResponse - 0.1))
828  {
829  norm += response;
830  accumulatedVarianceThTh += (math::Square(angle - bestAngle) * response);
831  }
832  }
833  assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
834 
835  if (norm > KT_TOLERANCE)
836  {
837  if (accumulatedVarianceThTh < KT_TOLERANCE)
838  {
839  accumulatedVarianceThTh = math::Square(searchAngleResolution);
840  }
841 
842  accumulatedVarianceThTh /= norm;
843  }
844  else
845  {
846  accumulatedVarianceThTh = 1000 * math::Square(searchAngleResolution);
847  }
848 
849  rCovariance(2, 2) = accumulatedVarianceThTh;
850  }
851 
858  {
859  m_pCorrelationGrid->Clear();
860 
861  // add all scans to grid
863  {
864  AddScan(*iter, viewPoint);
865  }
866  }
867 
874  void ScanMatcher::AddScan(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint, kt_bool doSmear)
875  {
876  PointVectorDouble validPoints = FindValidPoints(pScan, rViewPoint);
877 
878  // put in all valid points
879  const_forEach(PointVectorDouble, &validPoints)
880  {
881  Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(*iter);
882  if (!math::IsUpTo(gridPoint.GetX(), m_pCorrelationGrid->GetROI().GetWidth()) ||
883  !math::IsUpTo(gridPoint.GetY(), m_pCorrelationGrid->GetROI().GetHeight()))
884  {
885  // point not in grid
886  continue;
887  }
888 
889  int gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);
890 
891  // set grid cell as occupied
892  if (m_pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied)
893  {
894  // value already set
895  continue;
896  }
897 
898  m_pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied;
899 
900  // smear grid
901  if (doSmear == true)
902  {
903  m_pCorrelationGrid->SmearPoint(gridPoint);
904  }
905  }
906  }
907 
915  {
916  const PointVectorDouble& rPointReadings = pScan->GetPointReadings();
917 
918  // points must be at least 10 cm away when making comparisons of inside/outside of viewpoint
919  const kt_double minSquareDistance = math::Square(0.1); // in m^2
920 
921  // this iterator lags from the main iterator adding points only when the points are on
922  // the same side as the viewpoint
923  PointVectorDouble::const_iterator trailingPointIter = rPointReadings.begin();
924  PointVectorDouble validPoints;
925 
926  Vector2<kt_double> firstPoint;
927  kt_bool firstTime = true;
928  const_forEach(PointVectorDouble, &rPointReadings)
929  {
930  Vector2<kt_double> currentPoint = *iter;
931 
932  if (firstTime && !std::isnan(currentPoint.GetX()) && !std::isnan(currentPoint.GetY()))
933  {
934  firstPoint = currentPoint;
935  firstTime = false;
936  }
937 
938  Vector2<kt_double> delta = firstPoint - currentPoint;
939  if (delta.SquaredLength() > minSquareDistance)
940  {
941  // This compute the Determinant (viewPoint FirstPoint, viewPoint currentPoint)
942  // Which computes the direction of rotation, if the rotation is counterclock
943  // wise then we are looking at data we should keep. If it's negative rotation
944  // we should not included in in the matching
945  // have enough distance, check viewpoint
946  double a = rViewPoint.GetY() - firstPoint.GetY();
947  double b = firstPoint.GetX() - rViewPoint.GetX();
948  double c = firstPoint.GetY() * rViewPoint.GetX() - firstPoint.GetX() * rViewPoint.GetY();
949  double ss = currentPoint.GetX() * a + currentPoint.GetY() * b + c;
950 
951  // reset beginning point
952  firstPoint = currentPoint;
953 
954  if (ss < 0.0) // wrong side, skip and keep going
955  {
956  trailingPointIter = iter;
957  }
958  else
959  {
960  for (; trailingPointIter != iter; ++trailingPointIter)
961  {
962  validPoints.push_back(*trailingPointIter);
963  }
964  }
965  }
966  }
967 
968  return validPoints;
969  }
970 
977  kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const
978  {
979  kt_double response = 0.0;
980 
981  // add up value for each point
982  kt_int8u* pByte = m_pCorrelationGrid->GetDataPointer() + gridPositionIndex;
983 
984  const LookupArray* pOffsets = m_pGridLookup->GetLookupArray(angleIndex);
985  assert(pOffsets != NULL);
986 
987  // get number of points in offset list
988  kt_int32u nPoints = pOffsets->GetSize();
989  if (nPoints == 0)
990  {
991  return response;
992  }
993 
994  // calculate response
995  kt_int32s* pAngleIndexPointer = pOffsets->GetArrayPointer();
996  for (kt_int32u i = 0; i < nPoints; i++)
997  {
998  // ignore points that fall off the grid
999  kt_int32s pointGridIndex = gridPositionIndex + pAngleIndexPointer[i];
1000  if (!math::IsUpTo(pointGridIndex, m_pCorrelationGrid->GetDataSize()) || pAngleIndexPointer[i] == INVALID_SCAN)
1001  {
1002  continue;
1003  }
1004 
1005  // uses index offsets to efficiently find location of point in the grid
1006  response += pByte[pAngleIndexPointer[i]];
1007  }
1008 
1009  // normalize response
1010  response /= (nPoints * GridStates_Occupied);
1011  assert(fabs(response) <= 1.0);
1012 
1013  return response;
1014  }
1015 
1016 
1020 
1021  template<typename T>
1023  {
1024  public:
1029  : GraphTraversal<T>(pGraph)
1030  {
1031  }
1032 
1037  {
1038  }
1039 
1040  public:
1047  virtual std::vector<T*> Traverse(Vertex<T>* pStartVertex, Visitor<T>* pVisitor)
1048  {
1049  std::queue<Vertex<T>*> toVisit;
1050  std::set<Vertex<T>*> seenVertices;
1051  std::vector<Vertex<T>*> validVertices;
1052 
1053  toVisit.push(pStartVertex);
1054  seenVertices.insert(pStartVertex);
1055 
1056  do
1057  {
1058  Vertex<T>* pNext = toVisit.front();
1059  toVisit.pop();
1060 
1061  if (pVisitor->Visit(pNext))
1062  {
1063  // vertex is valid, explore neighbors
1064  validVertices.push_back(pNext);
1065 
1066  std::vector<Vertex<T>*> adjacentVertices = pNext->GetAdjacentVertices();
1067  forEach(typename std::vector<Vertex<T>*>, &adjacentVertices)
1068  {
1069  Vertex<T>* pAdjacent = *iter;
1070 
1071  // adjacent vertex has not yet been seen, add to queue for processing
1072  if (seenVertices.find(pAdjacent) == seenVertices.end())
1073  {
1074  toVisit.push(pAdjacent);
1075  seenVertices.insert(pAdjacent);
1076  }
1077  }
1078  }
1079  } while (toVisit.empty() == false);
1080 
1081  std::vector<T*> objects;
1082  forEach(typename std::vector<Vertex<T>*>, &validVertices)
1083  {
1084  objects.push_back((*iter)->GetObject());
1085  }
1086 
1087  return objects;
1088  }
1089  }; // class BreadthFirstTraversal
1090 
1094 
1095  class NearScanVisitor : public Visitor<LocalizedRangeScan>
1096  {
1097  public:
1098  NearScanVisitor(LocalizedRangeScan* pScan, kt_double maxDistance, kt_bool useScanBarycenter)
1099  : m_MaxDistanceSquared(math::Square(maxDistance))
1100  , m_UseScanBarycenter(useScanBarycenter)
1101  {
1102  m_CenterPose = pScan->GetReferencePose(m_UseScanBarycenter);
1103  }
1104 
1106  {
1107  LocalizedRangeScan* pScan = pVertex->GetObject();
1108 
1109  Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter);
1110 
1111  kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition());
1112  return (squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE);
1113  }
1114 
1115  protected:
1119  }; // NearScanVisitor
1120 
1124 
1125  MapperGraph::MapperGraph(Mapper* pMapper, kt_double rangeThreshold)
1126  : m_pMapper(pMapper)
1127  {
1131  assert(m_pLoopScanMatcher);
1132 
1134  }
1135 
1137  {
1138  delete m_pLoopScanMatcher;
1139  m_pLoopScanMatcher = NULL;
1140 
1141  delete m_pTraversal;
1142  m_pTraversal = NULL;
1143  }
1144 
1146  {
1147  assert(pScan);
1148 
1149  if (pScan != NULL)
1150  {
1153  if (m_pMapper->m_pScanOptimizer != NULL)
1154  {
1155  m_pMapper->m_pScanOptimizer->AddNode(pVertex);
1156  }
1157  }
1158  }
1159 
1160  void MapperGraph::AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance)
1161  {
1163 
1164  const Name& rSensorName = pScan->GetSensorName();
1165 
1166  // link to previous scan
1167  kt_int32s previousScanNum = pScan->GetStateId() - 1;
1168  if (pSensorManager->GetLastScan(rSensorName) != NULL)
1169  {
1170  assert(previousScanNum >= 0);
1171  LinkScans(pSensorManager->GetScan(rSensorName, previousScanNum), pScan, pScan->GetSensorPose(), rCovariance);
1172  }
1173 
1174  Pose2Vector means;
1175  std::vector<Matrix3> covariances;
1176 
1177  // first scan (link to first scan of other robots)
1178  if (pSensorManager->GetLastScan(rSensorName) == NULL)
1179  {
1180  assert(pSensorManager->GetScans(rSensorName).size() == 1);
1181 
1182  std::vector<Name> deviceNames = pSensorManager->GetSensorNames();
1183  forEach(std::vector<Name>, &deviceNames)
1184  {
1185  const Name& rCandidateSensorName = *iter;
1186 
1187  // skip if candidate device is the same or other device has no scans
1188  if ((rCandidateSensorName == rSensorName) || (pSensorManager->GetScans(rCandidateSensorName).empty()))
1189  {
1190  continue;
1191  }
1192 
1193  Pose2 bestPose;
1194  Matrix3 covariance;
1196  pSensorManager->GetScans(rCandidateSensorName),
1197  bestPose, covariance);
1198  LinkScans(pScan, pSensorManager->GetScan(rCandidateSensorName, 0), bestPose, covariance);
1199 
1200  // only add to means and covariances if response was high "enough"
1202  {
1203  means.push_back(bestPose);
1204  covariances.push_back(covariance);
1205  }
1206  }
1207  }
1208  else
1209  {
1210  // link to running scans
1211  Pose2 scanPose = pScan->GetSensorPose();
1212  means.push_back(scanPose);
1213  covariances.push_back(rCovariance);
1214  LinkChainToScan(pSensorManager->GetRunningScans(rSensorName), pScan, scanPose, rCovariance);
1215  }
1216 
1217  // link to other near chains (chains that include new scan are invalid)
1218  LinkNearChains(pScan, means, covariances);
1219 
1220  if (!means.empty())
1221  {
1222  pScan->SetSensorPose(ComputeWeightedMean(means, covariances));
1223  }
1224  }
1225 
1227  {
1228  kt_bool loopClosed = false;
1229 
1230  kt_int32u scanIndex = 0;
1231 
1232  LocalizedRangeScanVector candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
1233 
1234  while (!candidateChain.empty())
1235  {
1236  Pose2 bestPose;
1237  Matrix3 covariance;
1238  kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain,
1239  bestPose, covariance, false, false);
1240 
1241  std::stringstream stream;
1242  stream << "COARSE RESPONSE: " << coarseResponse
1243  << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")"
1244  << std::endl;
1245  stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1)
1246  << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")";
1247 
1248  m_pMapper->FireLoopClosureCheck(stream.str());
1249 
1250  if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
1251  (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
1252  (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()))
1253  {
1254  LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector());
1255  tmpScan.SetUniqueId(pScan->GetUniqueId());
1256  tmpScan.SetTime(pScan->GetTime());
1257  tmpScan.SetStateId(pScan->GetStateId());
1258  tmpScan.SetCorrectedPose(pScan->GetCorrectedPose());
1259  tmpScan.SetSensorPose(bestPose); // This also updates OdometricPose.
1260  kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, candidateChain,
1261  bestPose, covariance, false);
1262 
1263  std::stringstream stream1;
1264  stream1 << "FINE RESPONSE: " << fineResponse << " (>"
1265  << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl;
1266  m_pMapper->FireLoopClosureCheck(stream1.str());
1267 
1268  if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue())
1269  {
1270  m_pMapper->FireLoopClosureCheck("REJECTED!");
1271  }
1272  else
1273  {
1274  m_pMapper->FireBeginLoopClosure("Closing loop...");
1275 
1276  pScan->SetSensorPose(bestPose);
1277  LinkChainToScan(candidateChain, pScan, bestPose, covariance);
1278  CorrectPoses();
1279 
1280  m_pMapper->FireEndLoopClosure("Loop closed!");
1281 
1282  loopClosed = true;
1283  }
1284  }
1285 
1286  candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
1287  }
1288 
1289  return loopClosed;
1290  }
1291 
1293  const Pose2& rPose) const
1294  {
1295  LocalizedRangeScan* pClosestScan = NULL;
1296  kt_double bestSquaredDistance = DBL_MAX;
1297 
1299  {
1300  Pose2 scanPose = (*iter)->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
1301 
1302  kt_double squaredDistance = rPose.GetPosition().SquaredDistance(scanPose.GetPosition());
1303  if (squaredDistance < bestSquaredDistance)
1304  {
1305  bestSquaredDistance = squaredDistance;
1306  pClosestScan = *iter;
1307  }
1308  }
1309 
1310  return pClosestScan;
1311  }
1312 
1314  LocalizedRangeScan* pTargetScan, kt_bool& rIsNewEdge)
1315  {
1316  // check that vertex exists
1317  assert(pSourceScan->GetStateId() < (kt_int32s)m_Vertices[pSourceScan->GetSensorName()].size());
1318  assert(pTargetScan->GetStateId() < (kt_int32s)m_Vertices[pTargetScan->GetSensorName()].size());
1319 
1320  Vertex<LocalizedRangeScan>* v1 = m_Vertices[pSourceScan->GetSensorName()][pSourceScan->GetStateId()];
1321  Vertex<LocalizedRangeScan>* v2 = m_Vertices[pTargetScan->GetSensorName()][pTargetScan->GetStateId()];
1322 
1323  // see if edge already exists
1324  const_forEach(std::vector<Edge<LocalizedRangeScan>*>, &(v1->GetEdges()))
1325  {
1326  Edge<LocalizedRangeScan>* pEdge = *iter;
1327 
1328  if (pEdge->GetTarget() == v2)
1329  {
1330  rIsNewEdge = false;
1331  return pEdge;
1332  }
1333  }
1334 
1337  rIsNewEdge = true;
1338  return pEdge;
1339  }
1340 
1342  const Pose2& rMean, const Matrix3& rCovariance)
1343  {
1344  kt_bool isNewEdge = true;
1345  Edge<LocalizedRangeScan>* pEdge = AddEdge(pFromScan, pToScan, isNewEdge);
1346 
1347  // only attach link information if the edge is new
1348  if (isNewEdge == true)
1349  {
1350  pEdge->SetLabel(new LinkInfo(pFromScan->GetSensorPose(), rMean, rCovariance));
1351  if (m_pMapper->m_pScanOptimizer != NULL)
1352  {
1354  }
1355  }
1356  }
1357 
1358  void MapperGraph::LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector<Matrix3>& rCovariances)
1359  {
1360  const std::vector<LocalizedRangeScanVector> nearChains = FindNearChains(pScan);
1361  const_forEach(std::vector<LocalizedRangeScanVector>, &nearChains)
1362  {
1363  if (iter->size() < m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
1364  {
1365  continue;
1366  }
1367 
1368  Pose2 mean;
1369  Matrix3 covariance;
1370  // match scan against "near" chain
1371  kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false);
1373  {
1374  rMeans.push_back(mean);
1375  rCovariances.push_back(covariance);
1376  LinkChainToScan(*iter, pScan, mean, covariance);
1377  }
1378  }
1379  }
1380 
1382  const Pose2& rMean, const Matrix3& rCovariance)
1383  {
1385 
1386  LocalizedRangeScan* pClosestScan = GetClosestScanToPose(rChain, pose);
1387  assert(pClosestScan != NULL);
1388 
1389  Pose2 closestScanPose = pClosestScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
1390 
1391  kt_double squaredDistance = pose.GetPosition().SquaredDistance(closestScanPose.GetPosition());
1393  {
1394  LinkScans(pClosestScan, pScan, rMean, rCovariance);
1395  }
1396  }
1397 
1398  std::vector<LocalizedRangeScanVector> MapperGraph::FindNearChains(LocalizedRangeScan* pScan)
1399  {
1400  std::vector<LocalizedRangeScanVector> nearChains;
1401 
1403 
1404  // to keep track of which scans have been added to a chain
1405  LocalizedRangeScanVector processed;
1406 
1407  const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan,
1409  const_forEach(LocalizedRangeScanVector, &nearLinkedScans)
1410  {
1411  LocalizedRangeScan* pNearScan = *iter;
1412 
1413  if (pNearScan == pScan)
1414  {
1415  continue;
1416  }
1417 
1418  // scan has already been processed, skip
1419  if (find(processed.begin(), processed.end(), pNearScan) != processed.end())
1420  {
1421  continue;
1422  }
1423 
1424  processed.push_back(pNearScan);
1425 
1426  // build up chain
1427  kt_bool isValidChain = true;
1428  std::list<LocalizedRangeScan*> chain;
1429 
1430  // add scans before current scan being processed
1431  for (kt_int32s candidateScanNum = pNearScan->GetStateId() - 1; candidateScanNum >= 0; candidateScanNum--)
1432  {
1433  LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(),
1434  candidateScanNum);
1435 
1436  // chain is invalid--contains scan being added
1437  if (pCandidateScan == pScan)
1438  {
1439  isValidChain = false;
1440  }
1441 
1442  Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
1443  kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
1444 
1446  {
1447  chain.push_front(pCandidateScan);
1448  processed.push_back(pCandidateScan);
1449  }
1450  else
1451  {
1452  break;
1453  }
1454  }
1455 
1456  chain.push_back(pNearScan);
1457 
1458  // add scans after current scan being processed
1459  kt_int32u end = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(pNearScan->GetSensorName()).size());
1460  for (kt_int32u candidateScanNum = pNearScan->GetStateId() + 1; candidateScanNum < end; candidateScanNum++)
1461  {
1462  LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(),
1463  candidateScanNum);
1464 
1465  if (pCandidateScan == pScan)
1466  {
1467  isValidChain = false;
1468  }
1469 
1470  Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());;
1471  kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
1472 
1474  {
1475  chain.push_back(pCandidateScan);
1476  processed.push_back(pCandidateScan);
1477  }
1478  else
1479  {
1480  break;
1481  }
1482  }
1483 
1484  if (isValidChain)
1485  {
1486  // change list to vector
1487  LocalizedRangeScanVector tempChain;
1488  std::copy(chain.begin(), chain.end(), std::inserter(tempChain, tempChain.begin()));
1489  // add chain to collection
1490  nearChains.push_back(tempChain);
1491  }
1492  }
1493 
1494  return nearChains;
1495  }
1496 
1498  {
1499  NearScanVisitor* pVisitor = new NearScanVisitor(pScan, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue());
1500  LocalizedRangeScanVector nearLinkedScans = m_pTraversal->Traverse(GetVertex(pScan), pVisitor);
1501  delete pVisitor;
1502 
1503  return nearLinkedScans;
1504  }
1505 
1506  Pose2 MapperGraph::ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector<Matrix3>& rCovariances) const
1507  {
1508  assert(rMeans.size() == rCovariances.size());
1509 
1510  // compute sum of inverses and create inverse list
1511  std::vector<Matrix3> inverses;
1512  inverses.reserve(rCovariances.size());
1513 
1514  Matrix3 sumOfInverses;
1515  const_forEach(std::vector<Matrix3>, &rCovariances)
1516  {
1517  Matrix3 inverse = iter->Inverse();
1518  inverses.push_back(inverse);
1519 
1520  sumOfInverses += inverse;
1521  }
1522  Matrix3 inverseOfSumOfInverses = sumOfInverses.Inverse();
1523 
1524  // compute weighted mean
1525  Pose2 accumulatedPose;
1526  kt_double thetaX = 0.0;
1527  kt_double thetaY = 0.0;
1528 
1529  Pose2Vector::const_iterator meansIter = rMeans.begin();
1530  const_forEach(std::vector<Matrix3>, &inverses)
1531  {
1532  Pose2 pose = *meansIter;
1533  kt_double angle = pose.GetHeading();
1534  thetaX += cos(angle);
1535  thetaY += sin(angle);
1536 
1537  Matrix3 weight = inverseOfSumOfInverses * (*iter);
1538  accumulatedPose += weight * pose;
1539 
1540  ++meansIter;
1541  }
1542 
1543  thetaX /= rMeans.size();
1544  thetaY /= rMeans.size();
1545  accumulatedPose.SetHeading(atan2(thetaY, thetaX));
1546 
1547  return accumulatedPose;
1548  }
1549 
1551  const Name& rSensorName,
1552  kt_int32u& rStartNum)
1553  {
1554  LocalizedRangeScanVector chain; // return value
1555 
1557 
1558  // possible loop closure chain should not include close scans that have a
1559  // path of links to the scan of interest
1560  const LocalizedRangeScanVector nearLinkedScans =
1562 
1563  kt_int32u nScans = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size());
1564  for (; rStartNum < nScans; rStartNum++)
1565  {
1566  LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum);
1567 
1568  Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
1569 
1570  kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition());
1572  {
1573  // a linked scan cannot be in the chain
1574  if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end())
1575  {
1576  chain.clear();
1577  }
1578  else
1579  {
1580  chain.push_back(pCandidateScan);
1581  }
1582  }
1583  else
1584  {
1585  // return chain if it is long "enough"
1586  if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
1587  {
1588  return chain;
1589  }
1590  else
1591  {
1592  chain.clear();
1593  }
1594  }
1595  }
1596 
1597  return chain;
1598  }
1599 
1601  {
1602  // optimize scans!
1604  if (pSolver != NULL)
1605  {
1606  pSolver->Compute();
1607 
1609  {
1610  m_pMapper->m_pMapperSensorManager->GetScan(iter->first)->SetSensorPose(iter->second);
1611  }
1612 
1613  pSolver->Clear();
1614  }
1615  }
1616 
1620 
1625  : Module("Mapper")
1626  , m_Initialized(false)
1627  , m_pSequentialScanMatcher(NULL)
1628  , m_pMapperSensorManager(NULL)
1629  , m_pGraph(NULL)
1630  , m_pScanOptimizer(NULL)
1631  {
1633  }
1634 
1638  Mapper::Mapper(const std::string& rName)
1639  : Module(rName)
1640  , m_Initialized(false)
1641  , m_pSequentialScanMatcher(NULL)
1642  , m_pMapperSensorManager(NULL)
1643  , m_pGraph(NULL)
1644  , m_pScanOptimizer(NULL)
1645  {
1647  }
1648 
1653  {
1654  Reset();
1655 
1656  delete m_pMapperSensorManager;
1657  }
1658 
1660  {
1662  "UseScanMatching",
1663  "When set to true, the mapper will use a scan matching algorithm. "
1664  "In most real-world situations this should be set to true so that the "
1665  "mapper algorithm can correct for noise and errors in odometry and "
1666  "scan data. In some simulator environments where the simulated scan "
1667  "and odometry data are very accurate, the scan matching algorithm can "
1668  "produce worse results. In those cases set this to false to improve "
1669  "results.",
1670  true,
1672 
1674  "UseScanBarycenter",
1675  "Use the barycenter of scan endpoints to define distances between "
1676  "scans.",
1677  true, GetParameterManager());
1678 
1680  "MinimumTimeInterval",
1681  "Sets the minimum time between scans. If a new scan's time stamp is "
1682  "longer than MinimumTimeInterval from the previously processed scan, "
1683  "the mapper will use the data from the new scan. Otherwise, it will "
1684  "discard the new scan if it also does not meet the minimum travel "
1685  "distance and heading requirements. For performance reasons, it is "
1686  "generally it is a good idea to only process scans if a reasonable "
1687  "amount of time has passed. This parameter is particularly useful "
1688  "when there is a need to process scans while the robot is stationary.",
1689  3600, GetParameterManager());
1690 
1692  "MinimumTravelDistance",
1693  "Sets the minimum travel between scans. If a new scan's position is "
1694  "more than minimumTravelDistance from the previous scan, the mapper "
1695  "will use the data from the new scan. Otherwise, it will discard the "
1696  "new scan if it also does not meet the minimum change in heading "
1697  "requirement. For performance reasons, generally it is a good idea to "
1698  "only process scans if the robot has moved a reasonable amount.",
1699  0.2, GetParameterManager());
1700 
1702  "MinimumTravelHeading",
1703  "Sets the minimum heading change between scans. If a new scan's "
1704  "heading is more than MinimumTravelHeading from the previous scan, the "
1705  "mapper will use the data from the new scan. Otherwise, it will "
1706  "discard the new scan if it also does not meet the minimum travel "
1707  "distance requirement. For performance reasons, generally it is a good "
1708  "idea to only process scans if the robot has moved a reasonable "
1709  "amount.",
1711 
1713  "ScanBufferSize",
1714  "Scan buffer size is the length of the scan chain stored for scan "
1715  "matching. \"ScanBufferSize\" should be set to approximately "
1716  "\"ScanBufferMaximumScanDistance\" / \"MinimumTravelDistance\". The "
1717  "idea is to get an area approximately 20 meters long for scan "
1718  "matching. For example, if we add scans every MinimumTravelDistance == "
1719  "0.3 meters, then \"scanBufferSize\" should be 20 / 0.3 = 67.)",
1720  70, GetParameterManager());
1721 
1723  "ScanBufferMaximumScanDistance",
1724  "Scan buffer maximum scan distance is the maximum distance between the "
1725  "first and last scans in the scan chain stored for matching.",
1726  20.0, GetParameterManager());
1727 
1729  "LinkMatchMinimumResponseFine",
1730  "Scans are linked only if the correlation response value is greater "
1731  "than this value.",
1732  0.8, GetParameterManager());
1733 
1735  "LinkScanMaximumDistance",
1736  "Maximum distance between linked scans. Scans that are farther apart "
1737  "will not be linked regardless of the correlation response value.",
1738  10.0, GetParameterManager());
1739 
1741  "LoopSearchMaximumDistance",
1742  "Scans less than this distance from the current position will be "
1743  "considered for a match in loop closure.",
1744  4.0, GetParameterManager());
1745 
1747  "DoLoopClosing",
1748  "Enable/disable loop closure.",
1749  true, GetParameterManager());
1750 
1752  "LoopMatchMinimumChainSize",
1753  "When the loop closure detection finds a candidate it must be part of "
1754  "a large set of linked scans. If the chain of scans is less than this "
1755  "value we do not attempt to close the loop.",
1756  10, GetParameterManager());
1757 
1759  "LoopMatchMaximumVarianceCoarse",
1760  "The co-variance values for a possible loop closure have to be less "
1761  "than this value to consider a viable solution. This applies to the "
1762  "coarse search.",
1764 
1766  "LoopMatchMinimumResponseCoarse",
1767  "If response is larger then this, then initiate loop closure search at "
1768  "the coarse resolution.",
1769  0.8, GetParameterManager());
1770 
1772  "LoopMatchMinimumResponseFine",
1773  "If response is larger then this, then initiate loop closure search at "
1774  "the fine resolution.",
1775  0.8, GetParameterManager());
1776 
1778  // CorrelationParameters correlationParameters;
1779 
1781  "CorrelationSearchSpaceDimension",
1782  "The size of the search grid used by the matcher. The search grid will "
1783  "have the size CorrelationSearchSpaceDimension * "
1784  "CorrelationSearchSpaceDimension",
1785  0.3, GetParameterManager());
1786 
1788  "CorrelationSearchSpaceResolution",
1789  "The resolution (size of a grid cell) of the correlation grid.",
1790  0.01, GetParameterManager());
1791 
1793  "CorrelationSearchSpaceSmearDeviation",
1794  "The point readings are smeared by this value in X and Y to create a "
1795  "smoother response.",
1796  0.03, GetParameterManager());
1797 
1798 
1800  // CorrelationParameters loopCorrelationParameters;
1801 
1803  "LoopSearchSpaceDimension",
1804  "The size of the search grid used by the matcher.",
1805  8.0, GetParameterManager());
1806 
1808  "LoopSearchSpaceResolution",
1809  "The resolution (size of a grid cell) of the correlation grid.",
1810  0.05, GetParameterManager());
1811 
1813  "LoopSearchSpaceSmearDeviation",
1814  "The point readings are smeared by this value in X and Y to create a "
1815  "smoother response.",
1816  0.03, GetParameterManager());
1817 
1819  // ScanMatcherParameters;
1820 
1822  "DistanceVariancePenalty",
1823  "Variance of penalty for deviating from odometry when scan-matching. "
1824  "The penalty is a multiplier (less than 1.0) is a function of the "
1825  "delta of the scan position being tested and the odometric pose.",
1827 
1829  "AngleVariancePenalty",
1830  "See DistanceVariancePenalty.",
1832 
1834  "FineSearchAngleOffset",
1835  "The range of angles to search during a fine search.",
1837 
1839  "CoarseSearchAngleOffset",
1840  "The range of angles to search during a coarse search.",
1842 
1844  "CoarseAngleResolution",
1845  "Resolution of angles to search during a coarse search.",
1847 
1849  "MinimumAnglePenalty",
1850  "Minimum value of the angle penalty multiplier so scores do not become "
1851  "too small.",
1852  0.9, GetParameterManager());
1853 
1855  "MinimumDistancePenalty",
1856  "Minimum value of the distance penalty multiplier so scores do not "
1857  "become too small.",
1858  0.5, GetParameterManager());
1859 
1861  "UseResponseExpansion",
1862  "Whether to increase the search space if no good matches are initially "
1863  "found.",
1864  false, GetParameterManager());
1865  }
1866  /* Adding in getters and setters here for easy parameter access */
1867 
1868  // General Parameters
1869 
1871  {
1872  return static_cast<bool>(m_pUseScanMatching->GetValue());
1873  }
1874 
1876  {
1877  return static_cast<bool>(m_pUseScanBarycenter->GetValue());
1878  }
1879 
1881  {
1882  return static_cast<double>(m_pMinimumTimeInterval->GetValue());
1883  }
1884 
1886  {
1887  return static_cast<double>(m_pMinimumTravelDistance->GetValue());
1888  }
1889 
1891  {
1892  return math::RadiansToDegrees(static_cast<double>(m_pMinimumTravelHeading->GetValue()));
1893  }
1894 
1896  {
1897  return static_cast<int>(m_pScanBufferSize->GetValue());
1898  }
1899 
1901  {
1902  return static_cast<double>(m_pScanBufferMaximumScanDistance->GetValue());
1903  }
1904 
1906  {
1907  return static_cast<double>(m_pLinkMatchMinimumResponseFine->GetValue());
1908  }
1909 
1911  {
1912  return static_cast<double>(m_pLinkScanMaximumDistance->GetValue());
1913  }
1914 
1916  {
1917  return static_cast<double>(m_pLoopSearchMaximumDistance->GetValue());
1918  }
1919 
1921  {
1922  return static_cast<bool>(m_pDoLoopClosing->GetValue());
1923  }
1924 
1926  {
1927  return static_cast<int>(m_pLoopMatchMinimumChainSize->GetValue());
1928  }
1929 
1931  {
1932  return static_cast<double>(std::sqrt(m_pLoopMatchMaximumVarianceCoarse->GetValue()));
1933  }
1934 
1936  {
1937  return static_cast<double>(m_pLoopMatchMinimumResponseCoarse->GetValue());
1938  }
1939 
1941  {
1942  return static_cast<double>(m_pLoopMatchMinimumResponseFine->GetValue());
1943  }
1944 
1945  // Correlation Parameters - Correlation Parameters
1946 
1948  {
1949  return static_cast<double>(m_pCorrelationSearchSpaceDimension->GetValue());
1950  }
1951 
1953  {
1954  return static_cast<double>(m_pCorrelationSearchSpaceResolution->GetValue());
1955  }
1956 
1958  {
1959  return static_cast<double>(m_pCorrelationSearchSpaceSmearDeviation->GetValue());
1960  }
1961 
1962  // Correlation Parameters - Loop Correlation Parameters
1963 
1965  {
1966  return static_cast<double>(m_pLoopSearchSpaceDimension->GetValue());
1967  }
1968 
1970  {
1971  return static_cast<double>(m_pLoopSearchSpaceResolution->GetValue());
1972  }
1973 
1975  {
1976  return static_cast<double>(m_pLoopSearchSpaceSmearDeviation->GetValue());
1977  }
1978 
1979  // ScanMatcher Parameters
1980 
1982  {
1983  return std::sqrt(static_cast<double>(m_pDistanceVariancePenalty->GetValue()));
1984  }
1985 
1987  {
1988  return std::sqrt(static_cast<double>(m_pAngleVariancePenalty->GetValue()));
1989  }
1990 
1992  {
1993  return static_cast<double>(m_pFineSearchAngleOffset->GetValue());
1994  }
1995 
1997  {
1998  return static_cast<double>(m_pCoarseSearchAngleOffset->GetValue());
1999  }
2000 
2002  {
2003  return static_cast<double>(m_pCoarseAngleResolution->GetValue());
2004  }
2005 
2007  {
2008  return static_cast<double>(m_pMinimumAnglePenalty->GetValue());
2009  }
2010 
2012  {
2013  return static_cast<double>(m_pMinimumDistancePenalty->GetValue());
2014  }
2015 
2017  {
2018  return static_cast<bool>(m_pUseResponseExpansion->GetValue());
2019  }
2020 
2021  /* Setters for parameters */
2022  // General Parameters
2024  {
2026  }
2027 
2029  {
2031  }
2032 
2034  {
2036  }
2037 
2039  {
2041  }
2042 
2044  {
2046  }
2047 
2049  {
2051  }
2052 
2054  {
2056  }
2057 
2059  {
2061  }
2062 
2064  {
2066  }
2067 
2069  {
2071  }
2072 
2074  {
2076  }
2077 
2079  {
2081  }
2082 
2084  {
2086  }
2087 
2089  {
2091  }
2092 
2094  {
2096  }
2097 
2098  // Correlation Parameters - Correlation Parameters
2100  {
2102  }
2103 
2105  {
2107  }
2108 
2110  {
2112  }
2113 
2114 
2115  // Correlation Parameters - Loop Closure Parameters
2117  {
2119  }
2120 
2122  {
2124  }
2125 
2127  {
2129  }
2130 
2131 
2132  // Scan Matcher Parameters
2134  {
2136  }
2137 
2139  {
2141  }
2142 
2144  {
2146  }
2147 
2149  {
2151  }
2152 
2154  {
2156  }
2157 
2159  {
2161  }
2162 
2164  {
2166  }
2167 
2169  {
2171  }
2172 
2173 
2174 
2175  void Mapper::Initialize(kt_double rangeThreshold)
2176  {
2177  if (m_Initialized == false)
2178  {
2179  // create sequential scan and loop matcher
2184  rangeThreshold);
2185  assert(m_pSequentialScanMatcher);
2186 
2189 
2190  m_pGraph = new MapperGraph(this, rangeThreshold);
2191 
2192  m_Initialized = true;
2193  }
2194  }
2195 
2197  {
2198  delete m_pSequentialScanMatcher;
2199  m_pSequentialScanMatcher = NULL;
2200 
2201  delete m_pGraph;
2202  m_pGraph = NULL;
2203 
2204  delete m_pMapperSensorManager;
2205  m_pMapperSensorManager = NULL;
2206 
2207  m_Initialized = false;
2208  }
2209 
2211  {
2212  return true;
2213  }
2214 
2216  {
2217  if (pScan != NULL)
2218  {
2219  karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();
2220 
2221  // validate scan
2222  if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false)
2223  {
2224  return false;
2225  }
2226 
2227  if (m_Initialized == false)
2228  {
2229  // initialize mapper with range threshold from device
2230  Initialize(pLaserRangeFinder->GetRangeThreshold());
2231  }
2232 
2233  // get last scan
2235 
2236  // update scans corrected pose based on last correction
2237  if (pLastScan != NULL)
2238  {
2239  Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose());
2240  pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose()));
2241  }
2242 
2243  // test if scan is outside minimum boundary or if heading is larger then minimum heading
2244  if (!HasMovedEnough(pScan, pLastScan))
2245  {
2246  return false;
2247  }
2248 
2249  Matrix3 covariance;
2250  covariance.SetToIdentity();
2251 
2252  // correct scan (if not first scan)
2253  if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
2254  {
2255  Pose2 bestPose;
2258  bestPose,
2259  covariance);
2260  pScan->SetSensorPose(bestPose);
2261  }
2262 
2263  // add scan to buffer and assign id
2265 
2267  {
2268  // add to graph
2269  m_pGraph->AddVertex(pScan);
2270  m_pGraph->AddEdges(pScan, covariance);
2271 
2273 
2274  if (m_pDoLoopClosing->GetValue())
2275  {
2276  std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
2277  const_forEach(std::vector<Name>, &deviceNames)
2278  {
2279  m_pGraph->TryCloseLoop(pScan, *iter);
2280  }
2281  }
2282  }
2283 
2285 
2286  return true;
2287  }
2288 
2289  return false;
2290  }
2291 
2299  {
2300  // test if first scan
2301  if (pLastScan == NULL)
2302  {
2303  return true;
2304  }
2305 
2306  // test if enough time has passed
2307  kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime();
2308  if (timeInterval >= m_pMinimumTimeInterval->GetValue())
2309  {
2310  return true;
2311  }
2312 
2313  Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose());
2314  Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose());
2315 
2316  // test if we have turned enough
2317  kt_double deltaHeading = math::NormalizeAngle(scannerPose.GetHeading() - lastScannerPose.GetHeading());
2318  if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue())
2319  {
2320  return true;
2321  }
2322 
2323  // test if we have moved enough
2324  kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition());
2325  if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE)
2326  {
2327  return true;
2328  }
2329 
2330  return false;
2331  }
2332 
2338  {
2339  LocalizedRangeScanVector allScans;
2340 
2341  if (m_pMapperSensorManager != NULL)
2342  {
2343  allScans = m_pMapperSensorManager->GetAllScans();
2344  }
2345 
2346  return allScans;
2347  }
2348 
2354  {
2355  m_Listeners.push_back(pListener);
2356  }
2357 
2363  {
2364  std::vector<MapperListener*>::iterator iter = std::find(m_Listeners.begin(), m_Listeners.end(), pListener);
2365  if (iter != m_Listeners.end())
2366  {
2367  m_Listeners.erase(iter);
2368  }
2369  }
2370 
2371  void Mapper::FireInfo(const std::string& rInfo) const
2372  {
2373  const_forEach(std::vector<MapperListener*>, &m_Listeners)
2374  {
2375  (*iter)->Info(rInfo);
2376  }
2377  }
2378 
2379  void Mapper::FireDebug(const std::string& rInfo) const
2380  {
2381  const_forEach(std::vector<MapperListener*>, &m_Listeners)
2382  {
2383  MapperDebugListener* pListener = dynamic_cast<MapperDebugListener*>(*iter);
2384 
2385  if (pListener != NULL)
2386  {
2387  pListener->Debug(rInfo);
2388  }
2389  }
2390  }
2391 
2392  void Mapper::FireLoopClosureCheck(const std::string& rInfo) const
2393  {
2394  const_forEach(std::vector<MapperListener*>, &m_Listeners)
2395  {
2396  MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
2397 
2398  if (pListener != NULL)
2399  {
2400  pListener->LoopClosureCheck(rInfo);
2401  }
2402  }
2403  }
2404 
2405  void Mapper::FireBeginLoopClosure(const std::string& rInfo) const
2406  {
2407  const_forEach(std::vector<MapperListener*>, &m_Listeners)
2408  {
2409  MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
2410 
2411  if (pListener != NULL)
2412  {
2413  pListener->BeginLoopClosure(rInfo);
2414  }
2415  }
2416  }
2417 
2418  void Mapper::FireEndLoopClosure(const std::string& rInfo) const
2419  {
2420  const_forEach(std::vector<MapperListener*>, &m_Listeners)
2421  {
2422  MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
2423 
2424  if (pListener != NULL)
2425  {
2426  pListener->EndLoopClosure(rInfo);
2427  }
2428  }
2429  }
2430 
2431  void Mapper::SetScanSolver(ScanSolver* pScanOptimizer)
2432  {
2433  m_pScanOptimizer = pScanOptimizer;
2434  }
2435 
2437  {
2438  return m_pGraph;
2439  }
2440 
2442  {
2443  return m_pSequentialScanMatcher;
2444  }
2445 
2447  {
2448  return m_pGraph->GetLoopScanMatcher();
2449  }
2450 } // namespace karto
std::vector< LocalizedRangeScanVector > FindNearChains(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:1398
const Pose2 & GetOdometricPose() const
Definition: Karto.h:5189
virtual kt_bool Process(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:2215
std::map< Name, ScanManager * > ScanManagerMap
Definition: Mapper.h:1190
void InitializeParameters()
Definition: Mapper.cpp:1659
T GetHeight() const
Definition: Karto.h:1846
ScanMatcher * m_pSequentialScanMatcher
Definition: Mapper.h:1687
void RemoveListener(MapperListener *pListener)
Definition: Mapper.cpp:2362
double getParamMinimumAnglePenalty()
Definition: Mapper.cpp:2006
std::vector< Vertex< T > * > GetAdjacentVertices() const
Definition: Mapper.h:250
double getParamLinkScanMaximumDistance()
Definition: Mapper.cpp:1910
kt_int32s GetStateId() const
Definition: Karto.h:4829
RangeReadingsVector GetRangeReadingsVector() const
Definition: Karto.h:5011
#define DISTANCE_PENALTY_GAIN
Definition: Mapper.cpp:38
bool getParamDoLoopClosing()
Definition: Mapper.cpp:1920
int32_t kt_int32s
Definition: Types.h:51
virtual kt_bool Validate()
Definition: Karto.h:3926
void setParamCorrelationSearchSpaceResolution(double d)
Definition: Mapper.cpp:2104
void setParamLoopMatchMinimumChainSize(int i)
Definition: Mapper.cpp:2078
const kt_int32s INVALID_SCAN
Definition: Math.h:47
Parameter< kt_double > * m_pLinkScanMaximumDistance
Definition: Mapper.h:1773
kt_double GetHeading() const
Definition: Karto.h:2064
kt_double Round(kt_double value)
Definition: Math.h:87
T * GetObject() const
Definition: Mapper.h:241
const PointVectorDouble & GetPointReadings(kt_bool wantFiltered=false) const
Definition: Karto.h:5323
void LinkNearChains(LocalizedRangeScan *pScan, Pose2Vector &rMeans, std::vector< Matrix3 > &rCovariances)
Definition: Mapper.cpp:1358
virtual void AddNode(Vertex< LocalizedRangeScan > *)
Definition: Mapper.h:755
void SetUniqueId(kt_int32u uniqueId)
Definition: Karto.h:4856
Parameter< kt_double > * m_pCoarseAngleResolution
Definition: Mapper.h:1872
NearScanVisitor(LocalizedRangeScan *pScan, kt_double maxDistance, kt_bool useScanBarycenter)
Definition: Mapper.cpp:1098
LocalizedRangeScanVector & GetScans(const Name &rSensorName)
Definition: Mapper.cpp:245
Parameter< kt_double > * m_pLoopMatchMaximumVarianceCoarse
Definition: Mapper.h:1801
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1204
Parameter< kt_double > * m_pMinimumTravelHeading
Definition: Mapper.h:1743
virtual void EndLoopClosure(const std::string &)
Definition: Mapper.h:74
void SetValue(const T &rValue)
Definition: Karto.h:3141
void setParamUseScanMatching(bool b)
Definition: Mapper.cpp:2023
#define forEachAs(listtype, list, iter)
Definition: Macros.h:69
void AddEdge(Edge< T > *pEdge)
Definition: Mapper.h:481
int getParamLoopMatchMinimumChainSize()
Definition: Mapper.cpp:1925
void ComputeAngularCovariance(const Pose2 &rBestPose, kt_double bestResponse, const Pose2 &rSearchCenter, kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3 &rCovariance)
Definition: Mapper.cpp:799
Parameter< kt_double > * m_pMinimumAnglePenalty
Definition: Mapper.h:1876
std::vector< MapperListener * > m_Listeners
Definition: Mapper.h:1694
LocalizedRangeScanVector & GetScans()
Definition: Mapper.cpp:109
virtual void Clear()
Definition: Mapper.h:783
double getParamCoarseSearchAngleOffset()
Definition: Mapper.cpp:1996
kt_bool m_UseScanBarycenter
Definition: Mapper.cpp:1118
void FireDebug(const std::string &rInfo) const
Definition: Mapper.cpp:2379
kt_int32s * GetArrayPointer()
Definition: Karto.h:6310
const T & GetValue() const
Definition: Karto.h:3132
kt_bool HasMovedEnough(LocalizedRangeScan *pScan, LocalizedRangeScan *pLastScan) const
Definition: Mapper.cpp:2298
const T & GetY() const
Definition: Karto.h:975
Pose2 GetReferencePose(kt_bool useBarycenter) const
Definition: Karto.h:5249
uint8_t kt_int8u
Definition: Types.h:46
const T & Maximum(const T &value1, const T &value2)
Definition: Math.h:111
kt_double DegreesToRadians(kt_double degrees)
Definition: Math.h:56
double getParamCoarseAngleResolution()
Definition: Mapper.cpp:2001
friend class MapperGraph
Definition: Mapper.h:1504
std::vector< Name > GetSensorNames()
Definition: Mapper.h:1231
virtual ScanMatcher * GetSequentialScanMatcher() const
Definition: Mapper.cpp:2441
kt_double GetX() const
Definition: Karto.h:2010
double getParamMinimumDistancePenalty()
Definition: Mapper.cpp:2011
virtual void LoopClosureCheck(const std::string &)
Definition: Mapper.h:64
void SetX(const T &x)
Definition: Karto.h:966
Parameter< kt_double > * m_pLoopSearchSpaceSmearDeviation
Definition: Mapper.h:1856
void setParamDoLoopClosing(bool b)
Definition: Mapper.cpp:2073
virtual kt_bool Visit(Vertex< LocalizedRangeScan > *pVertex)
Definition: Mapper.cpp:1105
void FireEndLoopClosure(const std::string &rInfo) const
Definition: Mapper.cpp:2418
void AddVertex(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:1145
Parameter< kt_int32u > * m_pLoopMatchMinimumChainSize
Definition: Mapper.h:1794
void setParamCorrelationSearchSpaceSmearDeviation(double d)
Definition: Mapper.cpp:2109
Parameter< kt_double > * m_pCorrelationSearchSpaceResolution
Definition: Mapper.h:1828
void Initialize(kt_double rangeThreshold)
Definition: Mapper.cpp:2175
T GetWidth() const
Definition: Karto.h:1828
double getParamMinimumTimeInterval()
Definition: Mapper.cpp:1880
Parameter< kt_double > * m_pMinimumDistancePenalty
Definition: Mapper.h:1877
void setParamDistanceVariancePenalty(double d)
Definition: Mapper.cpp:2133
void AddVertex(const Name &rName, Vertex< T > *pVertex)
Definition: Mapper.h:472
void ComputePositionalCovariance(const Pose2 &rBestPose, kt_double bestResponse, const Pose2 &rSearchCenter, const Vector2< kt_double > &rSearchSpaceOffset, const Vector2< kt_double > &rSearchSpaceResolution, kt_double searchAngleResolution, Matrix3 &rCovariance)
Definition: Mapper.cpp:693
virtual void AddConstraint(Edge< LocalizedRangeScan > *)
Definition: Mapper.h:769
Parameter< kt_double > * m_pDistanceVariancePenalty
Definition: Mapper.h:1864
void SetStateId(kt_int32s stateId)
Definition: Karto.h:4838
Parameter< kt_bool > * m_pUseScanMatching
Definition: Mapper.h:1705
LocalizedRangeScanVector & GetRunningScans(const Name &rSensorName)
Definition: Mapper.cpp:255
#define forEach(listtype, list)
Definition: Macros.h:66
void setParamScanBufferSize(int i)
Definition: Mapper.cpp:2048
void AddScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:224
void setParamCorrelationSearchSpaceDimension(double d)
Definition: Mapper.cpp:2099
void SetSensorPose(const Pose2 &rScanPose)
Definition: Karto.h:5276
LocalizedRangeScan * GetLastScan()
Definition: Mapper.cpp:91
void setParamMinimumTimeInterval(double d)
Definition: Mapper.cpp:2033
void SetLastScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:215
LocalizedRangeScanVector m_RunningScans
Definition: Mapper.cpp:161
#define ANGLE_PENALTY_GAIN
Definition: Mapper.cpp:39
void LinkChainToScan(const LocalizedRangeScanVector &rChain, LocalizedRangeScan *pScan, const Pose2 &rMean, const Matrix3 &rCovariance)
Definition: Mapper.cpp:1381
void SetToIdentity()
Definition: Karto.h:2370
double getParamLoopMatchMinimumResponseCoarse()
Definition: Mapper.cpp:1935
void LinkScans(LocalizedRangeScan *pFromScan, LocalizedRangeScan *pToScan, const Pose2 &rMean, const Matrix3 &rCovariance)
Definition: Mapper.cpp:1341
MapperSensorManager * m_pMapperSensorManager
Definition: Mapper.h:1689
GraphTraversal< LocalizedRangeScan > * m_pTraversal
Definition: Mapper.h:708
double getParamLoopMatchMinimumResponseFine()
Definition: Mapper.cpp:1940
Parameter< kt_bool > * m_pDoLoopClosing
Definition: Mapper.h:1779
virtual kt_bool Visit(Vertex< T > *pVertex)=0
BreadthFirstTraversal(Graph< T > *pGraph)
Definition: Mapper.cpp:1028
void setParamAngleVariancePenalty(double d)
Definition: Mapper.cpp:2138
Parameter< kt_double > * m_pFineSearchAngleOffset
Definition: Mapper.h:1868
kt_int32u m_RunningBufferMaximumSize
Definition: Mapper.cpp:164
const T & GetX() const
Definition: Karto.h:957
kt_double RadiansToDegrees(kt_double radians)
Definition: Math.h:66
Parameter< kt_double > * m_pScanBufferMaximumScanDistance
Definition: Mapper.h:1760
ScanSolver * m_pScanOptimizer
Definition: Mapper.h:1692
Pose2 GetSensorPose() const
Definition: Karto.h:5267
void setParamLoopMatchMaximumVarianceCoarse(double d)
Definition: Mapper.cpp:2083
virtual void Debug(const std::string &)
Definition: Mapper.h:52
void AddListener(MapperListener *pListener)
Definition: Mapper.cpp:2353
Parameter< kt_double > * m_pLoopSearchSpaceDimension
Definition: Mapper.h:1844
void setParamLinkScanMaximumDistance(double d)
Definition: Mapper.cpp:2063
void SetLastScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:100
void setParamMinimumAnglePenalty(double d)
Definition: Mapper.cpp:2158
kt_double MatchScan(LocalizedRangeScan *pScan, const LocalizedRangeScanVector &rBaseScans, Pose2 &rMean, Matrix3 &rCovariance, kt_bool doPenalize=true, kt_bool doRefineMatch=true)
Definition: Mapper.cpp:362
virtual ParameterManager * GetParameterManager()
Definition: Karto.h:635
virtual const IdPoseVector & GetCorrections() const =0
double getParamCorrelationSearchSpaceSmearDeviation()
Definition: Mapper.cpp:1957
const Name & GetSensorName() const
Definition: Karto.h:4883
ScanManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
Definition: Mapper.cpp:54
ScanMatcher * GetLoopScanMatcher() const
Definition: Mapper.h:607
Parameter< kt_double > * m_pMinimumTravelDistance
Definition: Mapper.h:1733
void AddRunningScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:235
virtual ~Mapper()
Definition: Mapper.cpp:1652
void AddRunningScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:127
kt_int32s GetUniqueId() const
Definition: Karto.h:4847
Mapper * m_pMapper
Definition: Mapper.h:698
double getParamMinimumTravelHeading()
Definition: Mapper.cpp:1890
void setParamLoopSearchSpaceDimension(double d)
Definition: Mapper.cpp:2116
void setParamLoopSearchSpaceResolution(double d)
Definition: Mapper.cpp:2121
static Grid * CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
Definition: Karto.h:4373
void setParamScanBufferMaximumScanDistance(double d)
Definition: Mapper.cpp:2053
void setParamMinimumDistancePenalty(double d)
Definition: Mapper.cpp:2163
LocalizedRangeScanVector & GetRunningScans()
Definition: Mapper.cpp:118
double getParamMinimumTravelDistance()
Definition: Mapper.cpp:1885
PointVectorDouble FindValidPoints(LocalizedRangeScan *pScan, const Vector2< kt_double > &rViewPoint) const
Definition: Mapper.cpp:914
std::vector< Pose2 > Pose2Vector
Definition: Karto.h:2174
kt_double GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const
Definition: Mapper.cpp:977
double getParamLoopSearchSpaceSmearDeviation()
Definition: Mapper.cpp:1974
double getParamFineSearchAngleOffset()
Definition: Mapper.cpp:1991
void setParamLoopSearchMaximumDistance(double d)
Definition: Mapper.cpp:2068
void FireBeginLoopClosure(const std::string &rInfo) const
Definition: Mapper.cpp:2405
void setParamLoopMatchMinimumResponseCoarse(double d)
Definition: Mapper.cpp:2088
bool getParamUseScanBarycenter()
Definition: Mapper.cpp:1875
kt_double GetTime() const
Definition: Karto.h:4865
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:172
kt_double SquaredDistance(const Vector2 &rOther) const
Definition: Karto.h:1031
void AddScan(LocalizedRangeScan *pScan, kt_int32s uniqueId)
Definition: Mapper.cpp:74
void setParamMinimumTravelDistance(double d)
Definition: Mapper.cpp:2038
const kt_double KT_TOLERANCE
Definition: Math.h:41
kt_double NormalizeAngle(kt_double angle)
Definition: Math.h:182
LocalizedRangeScan * GetScan(const Name &rSensorName, kt_int32s scanIndex)
Definition: Mapper.cpp:187
bool getParamUseScanMatching()
Definition: Mapper.cpp:1870
#define const_forEach(listtype, list)
Definition: Macros.h:72
double getParamScanBufferMaximumScanDistance()
Definition: Mapper.cpp:1900
void SetScanSolver(ScanSolver *pSolver)
Definition: Mapper.cpp:2431
virtual void BeginLoopClosure(const std::string &)
Definition: Mapper.h:69
LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan *pScan, const Name &rSensorName, kt_int32u &rStartNum)
Definition: Mapper.cpp:1550
int getParamScanBufferSize()
Definition: Mapper.cpp:1895
bool getParamUseResponseExpansion()
Definition: Mapper.cpp:2016
double getParamCorrelationSearchSpaceDimension()
Definition: Mapper.cpp:1947
kt_int32u GetSize() const
Definition: Karto.h:6256
virtual const LocalizedRangeScanVector GetAllProcessedScans() const
Definition: Mapper.cpp:2337
GridIndexLookup< kt_int8u > * m_pGridLookup
Definition: Mapper.h:1176
virtual ~ScanMatcher()
Definition: Mapper.cpp:297
bool kt_bool
Definition: Types.h:64
static CorrelationGrid * CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution, kt_double smearDeviation)
Definition: Mapper.h:813
void AddScans(const LocalizedRangeScanVector &rScans, Vector2< kt_double > viewPoint)
Definition: Mapper.cpp:857
kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend)
Definition: Math.h:221
const kt_double KT_PI
Definition: Math.h:32
void AddScan(LocalizedRangeScan *pScan, const Vector2< kt_double > &rViewPoint, kt_bool doSmear=true)
Definition: Mapper.cpp:874
void setParamLinkMatchMinimumResponseFine(double d)
Definition: Mapper.cpp:2058
CorrelationGrid * m_pCorrelationGrid
Definition: Mapper.h:1173
kt_double CorrelateScan(LocalizedRangeScan *pScan, const Pose2 &rSearchCenter, const Vector2< kt_double > &rSearchSpaceOffset, const Vector2< kt_double > &rSearchSpaceResolution, kt_double searchAngleOffset, kt_double searchAngleResolution, kt_bool doPenalize, Pose2 &rMean, Matrix3 &rCovariance, kt_bool doingFineMatch)
Definition: Mapper.cpp:483
Parameter< kt_double > * m_pLoopSearchSpaceResolution
Definition: Mapper.h:1850
#define MAX_VARIANCE
Definition: Mapper.cpp:37
kt_bool m_Initialized
Definition: Mapper.h:1685
Parameter< kt_double > * m_pCorrelationSearchSpaceDimension
Definition: Mapper.h:1822
void setParamCoarseSearchAngleOffset(double d)
Definition: Mapper.cpp:2148
LaserRangeFinder * GetLaserRangeFinder() const
Definition: Karto.h:5063
double getParamDistanceVariancePenalty()
Definition: Mapper.cpp:1981
double getParamCorrelationSearchSpaceResolution()
Definition: Mapper.cpp:1952
void RegisterSensor(const Name &rSensorName)
Definition: Mapper.cpp:172
std::vector< std::pair< kt_int32s, Pose2 > > IdPoseVector
Definition: Mapper.h:724
void SetCorrectedPose(const Pose2 &rPose)
Definition: Karto.h:5220
LocalizedRangeScanVector GetAllScans()
Definition: Mapper.cpp:264
void setParamFineSearchAngleOffset(double d)
Definition: Mapper.cpp:2143
Pose2 GetSensorAt(const Pose2 &rPose) const
Definition: Karto.h:5297
Parameter< kt_double > * m_pLinkMatchMinimumResponseFine
Definition: Mapper.h:1766
Vertex< LocalizedRangeScan > * GetVertex(LocalizedRangeScan *pScan)
Definition: Mapper.h:618
T Square(T value)
Definition: Math.h:77
virtual ~ScanManager()
Definition: Mapper.cpp:64
double getParamAngleVariancePenalty()
Definition: Mapper.cpp:1986
static ScanMatcher * Create(Mapper *pMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold)
Definition: Mapper.cpp:304
kt_double SquaredLength() const
Definition: Karto.h:1013
ScanMatcher * m_pLoopScanMatcher
Definition: Mapper.h:703
void SetLabel(EdgeLabel *pLabel)
Definition: Mapper.h:359
Grid< kt_double > * m_pSearchSpaceProbs
Definition: Mapper.h:1174
kt_bool DoubleEqual(kt_double a, kt_double b)
Definition: Math.h:135
kt_double GetY() const
Definition: Karto.h:2028
Edge< LocalizedRangeScan > * AddEdge(LocalizedRangeScan *pSourceScan, LocalizedRangeScan *pTargetScan, kt_bool &rIsNewEdge)
Definition: Mapper.cpp:1313
void SetHeading(kt_double heading)
Definition: Karto.h:2073
LocalizedRangeScanVector FindNearLinkedScans(LocalizedRangeScan *pScan, kt_double maxDistance)
Definition: Mapper.cpp:1497
double kt_double
Definition: Types.h:67
double getParamLoopSearchSpaceResolution()
Definition: Mapper.cpp:1969
Parameter< kt_double > * m_pCorrelationSearchSpaceSmearDeviation
Definition: Mapper.h:1834
virtual std::vector< T * > Traverse(Vertex< T > *pStartVertex, Visitor< T > *pVisitor)
Definition: Mapper.cpp:1047
Definition: Karto.h:73
void setParamMinimumTravelHeading(double d)
Definition: Mapper.cpp:2043
void AddEdges(LocalizedRangeScan *pScan, const Matrix3 &rCovariance)
Definition: Mapper.cpp:1160
double getParamLoopSearchMaximumDistance()
Definition: Mapper.cpp:1915
void setParamUseResponseExpansion(bool b)
Definition: Mapper.cpp:2168
void Reset()
Definition: Mapper.cpp:2196
Pose2 ComputeWeightedMean(const Pose2Vector &rMeans, const std::vector< Matrix3 > &rCovariances) const
Definition: Mapper.cpp:1506
void FireLoopClosureCheck(const std::string &rInfo) const
Definition: Mapper.cpp:2392
Vertex< T > * GetTarget() const
Definition: Mapper.h:341
virtual void Compute()=0
MapperGraph * m_pGraph
Definition: Mapper.h:1691
Parameter< kt_double > * m_pLoopMatchMinimumResponseCoarse
Definition: Mapper.h:1807
double getParamLoopMatchMaximumVarianceCoarse()
Definition: Mapper.cpp:1930
kt_bool TryCloseLoop(LocalizedRangeScan *pScan, const Name &rSensorName)
Definition: Mapper.cpp:1226
virtual ScanMatcher * GetLoopScanMatcher() const
Definition: Mapper.cpp:2446
LocalizedRangeScan * m_pLastScan
Definition: Mapper.cpp:162
double getParamLinkMatchMinimumResponseFine()
Definition: Mapper.cpp:1905
virtual ~MapperGraph()
Definition: Mapper.cpp:1136
kt_double m_RunningBufferMaximumDistance
Definition: Mapper.cpp:165
Matrix3 Inverse() const
Definition: Karto.h:2448
Parameter< kt_double > * m_pCoarseSearchAngleOffset
Definition: Mapper.h:1869
void setParamLoopMatchMinimumResponseFine(double d)
Definition: Mapper.cpp:2093
const Vector2< kt_double > & GetPosition() const
Definition: Karto.h:2046
Parameter< kt_double > * m_pLoopSearchMaximumDistance
Definition: Mapper.h:1786
virtual MapperGraph * GetGraph() const
Definition: Mapper.cpp:2436
LocalizedRangeScan * GetClosestScanToPose(const LocalizedRangeScanVector &rScans, const Pose2 &rPose) const
Definition: Mapper.cpp:1292
void setParamUseScanBarycenter(bool b)
Definition: Mapper.cpp:2028
Parameter< kt_double > * m_pAngleVariancePenalty
Definition: Mapper.h:1865
void SetY(const T &y)
Definition: Karto.h:984
kt_double GetRangeThreshold() const
Definition: Karto.h:3781
kt_int32u GetNumberOfRangeReadings() const
Definition: Karto.h:5072
MapperGraph(Mapper *pMapper, kt_double rangeThreshold)
Definition: Mapper.cpp:1125
uint32_t kt_int32u
Definition: Types.h:52
double getParamLoopSearchSpaceDimension()
Definition: Mapper.cpp:1964
Parameter< kt_double > * m_pMinimumTimeInterval
Definition: Mapper.h:1723
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5211
Parameter< kt_bool > * m_pUseResponseExpansion
Definition: Mapper.h:1880
Parameter< kt_int32u > * m_pScanBufferSize
Definition: Mapper.h:1753
kt_bool IsUpTo(const T &value, const T &maximum)
Definition: Math.h:147
LocalizedRangeScan * GetLastScan(const Name &rSensorName)
Definition: Mapper.cpp:204
void setParamLoopSearchSpaceSmearDeviation(double d)
Definition: Mapper.cpp:2126
void FireInfo(const std::string &rInfo) const
Definition: Mapper.cpp:2371
std::vector< LocalizedRangeScan * > LocalizedRangeScanVector
Definition: Karto.h:5462
Parameter< kt_bool > * m_pUseScanBarycenter
Definition: Mapper.h:1710
LocalizedRangeScanVector m_Scans
Definition: Mapper.cpp:160
Parameter< kt_double > * m_pLoopMatchMinimumResponseFine
Definition: Mapper.h:1813
void setParamCoarseAngleResolution(double d)
Definition: Mapper.cpp:2153
kt_double m_MaxDistanceSquared
Definition: Mapper.cpp:1117


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