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 #include <karto_sdk/Types.h>
26 #include <math.h>
27 #include <assert.h>
28 #include <boost/serialization/vector.hpp>
29 
30 #include "karto_sdk/Mapper.h"
31 
40 namespace karto
41 {
42 
43  // enable this for verbose debug information
44  // #define KARTO_DEBUG
45 
46  #define MAX_VARIANCE 500.0
47  #define DISTANCE_PENALTY_GAIN 0.2
48  #define ANGLE_PENALTY_GAIN 0.2
49 
53 
58  {
59  public:
63  ScanManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
64  : m_pLastScan(NULL)
65  , m_RunningBufferMaximumSize(runningBufferMaximumSize)
66  , m_RunningBufferMaximumDistance(runningBufferMaximumDistance)
67  , m_NextStateId(0)
68  {
69  }
70 
72 
76  virtual ~ScanManager()
77  {
78  Clear();
79  }
80 
81  public:
86  inline void AddScan(LocalizedRangeScan* pScan, kt_int32s uniqueId)
87  {
88  // assign state id to scan
89  pScan->SetStateId(m_NextStateId);
90 
91  // assign unique id to scan
92  pScan->SetUniqueId(uniqueId);
93 
94  // add it to scan buffer
95  m_Scans.insert({pScan->GetStateId(), pScan});
96  m_NextStateId++;
97  }
98 
105  {
106  return m_pLastScan;
107  }
108 
114  {
115  m_pLastScan = pScan;
116  }
117 
123  {
124  return m_Scans;
125  }
126 
132  {
133  return m_RunningScans;
134  }
135 
141  {
143  }
144 
150  {
151  m_RunningScans.push_back(pScan);
152 
153  // vector has at least one element (first line of this function), so this is valid
154  Pose2 frontScanPose = m_RunningScans.front()->GetSensorPose();
155  Pose2 backScanPose = m_RunningScans.back()->GetSensorPose();
156 
157  // cap vector size and remove all scans from front of vector that are too far from end of vector
158  kt_double squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
159  while (m_RunningScans.size() > m_RunningBufferMaximumSize ||
161  {
162  // remove front of running scans
163  m_RunningScans.erase(m_RunningScans.begin());
164 
165  // recompute stats of running scans
166  frontScanPose = m_RunningScans.front()->GetSensorPose();
167  backScanPose = m_RunningScans.back()->GetSensorPose();
168  squaredDistance = frontScanPose.GetPosition().SquaredDistance(backScanPose.GetPosition());
169  }
170  }
171 
177  {
178  LocalizedRangeScanMap::iterator it = m_Scans.find(pScan->GetStateId());
179  if (it != m_Scans.end())
180  {
181  it->second = NULL;
182  m_Scans.erase(it);
183  }
184  else
185  {
186  std::cout << "Remove Scan: Failed to find scan in m_Scans" << std::endl;
187  }
188  }
189 
194  {
195  m_RunningScans.clear();
196  }
197 
201  void Clear()
202  {
203  m_Scans.clear();
204  m_RunningScans.clear();
205  }
206 
207  private:
209  template<class Archive>
210  void serialize(Archive &ar, const unsigned int version)
211  {
212  ar & BOOST_SERIALIZATION_NVP(m_Scans);
213  ar & BOOST_SERIALIZATION_NVP(m_RunningScans);
214  ar & BOOST_SERIALIZATION_NVP(m_pLastScan);
215  ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumSize);
216  ar & BOOST_SERIALIZATION_NVP(m_RunningBufferMaximumDistance);
217  ar & BOOST_SERIALIZATION_NVP(m_NextStateId);
218  }
219 
220  private:
225 
228  }; // ScanManager
229 
233 
234  void MapperSensorManager::RegisterSensor(const Name& rSensorName)
235  {
236  if (GetScanManager(rSensorName) == NULL)
237  {
238  m_ScanManagers[rSensorName] = new ScanManager(m_RunningBufferMaximumSize, m_RunningBufferMaximumDistance);
239  }
240  }
241 
242 
250  {
251  ScanManager* pScanManager = GetScanManager(rSensorName);
252  if (pScanManager != NULL)
253  {
254  LocalizedRangeScanMap::iterator it = pScanManager->GetScans().find(scanIndex);
255  if (it != pScanManager->GetScans().end())
256  {
257  return it->second;
258  }
259  else
260  {
261  return nullptr;
262  }
263  }
264 
265  assert(false);
266  return NULL;
267  }
268 
275  {
276  RegisterSensor(rSensorName);
277 
278  return GetScanManager(rSensorName)->GetLastScan();
279  }
280 
286  {
287  GetScanManager(pScan)->SetLastScan(pScan);
288  }
289 
295  {
296  GetScanManager(pScan)->AddScan(pScan, m_NextScanId);
297  m_Scans.insert({m_NextScanId, pScan});
298  m_NextScanId++;
299  }
300 
306  {
307  GetScanManager(pScan)->AddRunningScan(pScan);
308  }
309 
315  {
316  GetScanManager(pScan)->RemoveScan(pScan);
317 
318  LocalizedRangeScanMap::iterator it = m_Scans.find(pScan->GetStateId());
319  if (it != m_Scans.end())
320  {
321  it->second = NULL;
322  m_Scans.erase(it);
323  }
324  else
325  {
326  std::cout << "RemoveScan: Failed to find scan in m_Scans" << std::endl;
327  }
328  }
329 
336  {
337  return GetScanManager(rSensorName)->GetScans();
338  }
339 
346  {
347  return GetScanManager(rSensorName)->GetRunningScans();
348  }
349 
351  {
352  GetScanManager(rSensorName)->ClearRunningScans();
353  return;
354  }
355 
357  {
358  return GetScanManager(rSensorName)->GetRunningScanBufferSize();
359  }
360 
366  {
368 
369  forEach(ScanManagerMap, &m_ScanManagers)
370  {
371  LocalizedRangeScanMap& rScans = iter->second->GetScans();
372 
373  LocalizedRangeScanMap::iterator it;
374  for (it = rScans.begin(); it != rScans.end(); ++it)
375  {
376  scans.push_back(it->second);
377  }
378  }
379 
380  return scans;
381  }
382 
387  {
388 // SensorManager::Clear();
389 
390  forEach(ScanManagerMap, &m_ScanManagers)
391  {
392  delete iter->second;
393  iter->second = nullptr;
394  }
395 
396  m_ScanManagers.clear();
397  }
398 
402 
404  {
405  if (m_pCorrelationGrid)
406  {
407  delete m_pCorrelationGrid;
408  }
409  if (m_pSearchSpaceProbs)
410  {
411  delete m_pSearchSpaceProbs;
412  }
413  if (m_pGridLookup)
414  {
415  delete m_pGridLookup;
416  }
417 
418  }
419 
420  ScanMatcher* ScanMatcher::Create(Mapper* pMapper, kt_double searchSize, kt_double resolution,
421  kt_double smearDeviation, kt_double rangeThreshold)
422  {
423  // invalid parameters
424  if (resolution <= 0)
425  {
426  return NULL;
427  }
428  if (searchSize <= 0)
429  {
430  return NULL;
431  }
432  if (smearDeviation < 0)
433  {
434  return NULL;
435  }
436  if (rangeThreshold <= 0)
437  {
438  return NULL;
439  }
440 
441  assert(math::DoubleEqual(math::Round(searchSize / resolution), (searchSize / resolution)));
442 
443  // calculate search space in grid coordinates
444  kt_int32u searchSpaceSideSize = static_cast<kt_int32u>(math::Round(searchSize / resolution) + 1);
445 
446  // compute requisite size of correlation grid (pad grid so that scan points can't fall off the grid
447  // if a scan is on the border of the search space)
448  kt_int32u pointReadingMargin = static_cast<kt_int32u>(ceil(rangeThreshold / resolution));
449 
450  kt_int32s gridSize = searchSpaceSideSize + 2 * pointReadingMargin;
451 
452  // create correlation grid
453  assert(gridSize % 2 == 1);
454  CorrelationGrid* pCorrelationGrid = CorrelationGrid::CreateGrid(gridSize, gridSize, resolution, smearDeviation);
455 
456  // create search space probabilities
457  Grid<kt_double>* pSearchSpaceProbs = Grid<kt_double>::CreateGrid(searchSpaceSideSize,
458  searchSpaceSideSize, resolution);
459 
460  ScanMatcher* pScanMatcher = new ScanMatcher(pMapper);
461  pScanMatcher->m_pCorrelationGrid = pCorrelationGrid;
462  pScanMatcher->m_pSearchSpaceProbs = pSearchSpaceProbs;
463  pScanMatcher->m_pGridLookup = new GridIndexLookup<kt_int8u>(pCorrelationGrid);
464 
465  return pScanMatcher;
466  }
467 
478  template<class T>
479  kt_double ScanMatcher::MatchScan(LocalizedRangeScan* pScan, const T& rBaseScans, Pose2& rMean,
480  Matrix3& rCovariance, kt_bool doPenalize, kt_bool doRefineMatch)
481  {
483  // set scan pose to be center of grid
484 
485  // 1. get scan position
486  Pose2 scanPose = pScan->GetSensorPose();
487 
488  // scan has no readings; cannot do scan matching
489  // best guess of pose is based off of adjusted odometer reading
490  if (pScan->GetNumberOfRangeReadings() == 0)
491  {
492  rMean = scanPose;
493 
494  // maximum covariance
495  rCovariance(0, 0) = MAX_VARIANCE; // XX
496  rCovariance(1, 1) = MAX_VARIANCE; // YY
497  rCovariance(2, 2) = 4 * math::Square(m_pMapper->m_pCoarseAngleResolution->GetValue()); // TH*TH
498 
499  return 0.0;
500  }
501 
502  // 2. get size of grid
503  Rectangle2<kt_int32s> roi = m_pCorrelationGrid->GetROI();
504 
505  // 3. compute offset (in meters - lower left corner)
506  Vector2<kt_double> offset;
507  offset.SetX(scanPose.GetX() - (0.5 * (roi.GetWidth() - 1) * m_pCorrelationGrid->GetResolution()));
508  offset.SetY(scanPose.GetY() - (0.5 * (roi.GetHeight() - 1) * m_pCorrelationGrid->GetResolution()));
509 
510  // 4. set offset
511  m_pCorrelationGrid->GetCoordinateConverter()->SetOffset(offset);
512 
514 
515  // set up correlation grid
516  AddScans(rBaseScans, scanPose.GetPosition());
517 
518  // compute how far to search in each direction
519  Vector2<kt_double> searchDimensions(m_pSearchSpaceProbs->GetWidth(), m_pSearchSpaceProbs->GetHeight());
520  Vector2<kt_double> coarseSearchOffset(0.5 * (searchDimensions.GetX() - 1) * m_pCorrelationGrid->GetResolution(),
521  0.5 * (searchDimensions.GetY() - 1) * m_pCorrelationGrid->GetResolution());
522 
523  // a coarse search only checks half the cells in each dimension
524  Vector2<kt_double> coarseSearchResolution(2 * m_pCorrelationGrid->GetResolution(),
525  2 * m_pCorrelationGrid->GetResolution());
526 
527  // actual scan-matching
528  kt_double bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
529  m_pMapper->m_pCoarseSearchAngleOffset->GetValue(),
530  m_pMapper->m_pCoarseAngleResolution->GetValue(),
531  doPenalize, rMean, rCovariance, false);
532 
533  if (m_pMapper->m_pUseResponseExpansion->GetValue() == true)
534  {
535  if (math::DoubleEqual(bestResponse, 0.0))
536  {
537 #ifdef KARTO_DEBUG
538  std::cout << "Mapper Info: Expanding response search space!" << std::endl;
539 #endif
540  // try and increase search angle offset with 20 degrees and do another match
541  kt_double newSearchAngleOffset = m_pMapper->m_pCoarseSearchAngleOffset->GetValue();
542  for (kt_int32u i = 0; i < 3; i++)
543  {
544  newSearchAngleOffset += math::DegreesToRadians(20);
545 
546  bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
547  newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(),
548  doPenalize, rMean, rCovariance, false);
549 
550  if (math::DoubleEqual(bestResponse, 0.0) == false)
551  {
552  break;
553  }
554  }
555 
556 #ifdef KARTO_DEBUG
557  if (math::DoubleEqual(bestResponse, 0.0))
558  {
559  std::cout << "Mapper Warning: Unable to calculate response!" << std::endl;
560  }
561 #endif
562  }
563  }
564 
565  if (doRefineMatch)
566  {
567  Vector2<kt_double> fineSearchOffset(coarseSearchResolution * 0.5);
568  Vector2<kt_double> fineSearchResolution(m_pCorrelationGrid->GetResolution(), m_pCorrelationGrid->GetResolution());
569  bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution,
570  0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(),
571  m_pMapper->m_pFineSearchAngleOffset->GetValue(),
572  doPenalize, rMean, rCovariance, true);
573  }
574 
575 #ifdef KARTO_DEBUG
576  std::cout << " BEST POSE = " << rMean << " BEST RESPONSE = " << bestResponse << ", VARIANCE = "
577  << rCovariance(0, 0) << ", " << rCovariance(1, 1) << std::endl;
578 #endif
579  assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
580 
581  return bestResponse;
582  }
583 
584  void ScanMatcher::operator() (const kt_double& y) const
585  {
586  kt_int32u poseResponseCounter;
587  kt_int32u x_pose;
588  kt_int32u y_pose = std::find(m_yPoses.begin(), m_yPoses.end(), y) - m_yPoses.begin();
589 
590  const kt_int32u size_x = m_xPoses.size();
591 
592  kt_double newPositionY = m_rSearchCenter.GetY() + y;
593  kt_double squareY = math::Square(y);
594 
595  for ( std::vector<kt_double>::const_iterator xIter = m_xPoses.begin(); xIter != m_xPoses.end(); ++xIter )
596  {
597  x_pose = std::distance(m_xPoses.begin(), xIter);
598  kt_double x = *xIter;
599  kt_double newPositionX = m_rSearchCenter.GetX() + x;
600  kt_double squareX = math::Square(x);
601 
602  Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(Vector2<kt_double>(newPositionX, newPositionY));
603  kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);
604  assert(gridIndex >= 0);
605 
606  kt_double angle = 0.0;
607  kt_double startAngle = m_rSearchCenter.GetHeading() - m_searchAngleOffset;
608  for (kt_int32u angleIndex = 0; angleIndex < m_nAngles; angleIndex++)
609  {
610  angle = startAngle + angleIndex * m_searchAngleResolution;
611 
612  kt_double response = GetResponse(angleIndex, gridIndex);
613  if (m_doPenalize && (math::DoubleEqual(response, 0.0) == false))
614  {
615  // simple model (approximate Gaussian) to take odometry into account
616  kt_double squaredDistance = squareX + squareY;
617  kt_double distancePenalty = 1.0 - (DISTANCE_PENALTY_GAIN *
618  squaredDistance / m_pMapper->m_pDistanceVariancePenalty->GetValue());
619  distancePenalty = math::Maximum(distancePenalty, m_pMapper->m_pMinimumDistancePenalty->GetValue());
620 
621  kt_double squaredAngleDistance = math::Square(angle - m_rSearchCenter.GetHeading());
622  kt_double anglePenalty = 1.0 - (ANGLE_PENALTY_GAIN *
623  squaredAngleDistance / m_pMapper->m_pAngleVariancePenalty->GetValue());
624  anglePenalty = math::Maximum(anglePenalty, m_pMapper->m_pMinimumAnglePenalty->GetValue());
625 
626  response *= (distancePenalty * anglePenalty);
627  }
628 
629  // store response and pose
630  poseResponseCounter = (y_pose*size_x + x_pose)*(m_nAngles) + angleIndex;
631  m_pPoseResponse[poseResponseCounter] = std::pair<kt_double, Pose2>(response, Pose2(newPositionX, newPositionY,
632  math::NormalizeAngle(angle)));
633  }
634  }
635  return;
636  }
637 
655  const Vector2<kt_double>& rSearchSpaceOffset,
656  const Vector2<kt_double>& rSearchSpaceResolution,
657  kt_double searchAngleOffset, kt_double searchAngleResolution,
658  kt_bool doPenalize, Pose2& rMean, Matrix3& rCovariance, kt_bool doingFineMatch)
659  {
660  assert(searchAngleResolution != 0.0);
661 
662  // setup lookup arrays
663  m_pGridLookup->ComputeOffsets(pScan, rSearchCenter.GetHeading(), searchAngleOffset, searchAngleResolution);
664 
665  // only initialize probability grid if computing positional covariance (during coarse match)
666  if (!doingFineMatch)
667  {
668  m_pSearchSpaceProbs->Clear();
669 
670  // position search grid - finds lower left corner of search grid
671  Vector2<kt_double> offset(rSearchCenter.GetPosition() - rSearchSpaceOffset);
672  m_pSearchSpaceProbs->GetCoordinateConverter()->SetOffset(offset);
673  }
674 
675  // calculate position arrays
676 
677  m_xPoses.clear();
678  kt_int32u nX = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetX() *
679  2.0 / rSearchSpaceResolution.GetX()) + 1);
680  kt_double startX = -rSearchSpaceOffset.GetX();
681  for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
682  {
683  m_xPoses.push_back(startX + xIndex * rSearchSpaceResolution.GetX());
684  }
685  assert(math::DoubleEqual(m_xPoses.back(), -startX));
686 
687  m_yPoses.clear();
688  kt_int32u nY = static_cast<kt_int32u>(math::Round(rSearchSpaceOffset.GetY() *
689  2.0 / rSearchSpaceResolution.GetY()) + 1);
690  kt_double startY = -rSearchSpaceOffset.GetY();
691  for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
692  {
693  m_yPoses.push_back(startY + yIndex * rSearchSpaceResolution.GetY());
694  }
695  assert(math::DoubleEqual(m_yPoses.back(), -startY));
696 
697  // calculate pose response array size
698  kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2.0 / searchAngleResolution) + 1);
699 
700  kt_int32u poseResponseSize = static_cast<kt_int32u>(m_xPoses.size() * m_yPoses.size() * nAngles);
701 
702  // allocate array
703  m_pPoseResponse = new std::pair<kt_double, Pose2>[poseResponseSize];
704 
705  Vector2<kt_int32s> startGridPoint = m_pCorrelationGrid->WorldToGrid(Vector2<kt_double>(rSearchCenter.GetX()
706  + startX, rSearchCenter.GetY() + startY));
707 
708  // this isn't good but its the fastest way to iterate. Should clean up later.
709  m_rSearchCenter = rSearchCenter;
710  m_searchAngleOffset = searchAngleOffset;
711  m_nAngles = nAngles;
712  m_searchAngleResolution = searchAngleResolution;
713  m_doPenalize = doPenalize;
714  tbb::parallel_do(m_yPoses, (*this) );
715 
716  // find value of best response (in [0; 1])
717  kt_double bestResponse = -1;
718  for (kt_int32u i = 0; i < poseResponseSize; i++)
719  {
720  bestResponse = math::Maximum(bestResponse, m_pPoseResponse[i].first);
721 
722  // will compute positional covariance, save best relative probability for each cell
723  if (!doingFineMatch)
724  {
725  const Pose2& rPose = m_pPoseResponse[i].second;
726  Vector2<kt_int32s> grid = m_pSearchSpaceProbs->WorldToGrid(rPose.GetPosition());
727  kt_double* ptr;
728 
729  try
730  {
731  ptr = (kt_double*)(m_pSearchSpaceProbs->GetDataPointer(grid));
732  }
733  catch(...)
734  {
735  throw std::runtime_error("Mapper FATAL ERROR - unable to get pointer in probability search!");
736  }
737 
738  if (ptr == NULL)
739  {
740  throw std::runtime_error("Mapper FATAL ERROR - Index out of range in probability search!");
741  }
742 
743  *ptr = math::Maximum(m_pPoseResponse[i].first, *ptr);
744  }
745  }
746 
747  // average all poses with same highest response
748  Vector2<kt_double> averagePosition;
749  kt_double thetaX = 0.0;
750  kt_double thetaY = 0.0;
751  kt_int32s averagePoseCount = 0;
752  for (kt_int32u i = 0; i < poseResponseSize; i++)
753  {
754  if (math::DoubleEqual(m_pPoseResponse[i].first, bestResponse))
755  {
756  averagePosition += m_pPoseResponse[i].second.GetPosition();
757 
758  kt_double heading = m_pPoseResponse[i].second.GetHeading();
759  thetaX += cos(heading);
760  thetaY += sin(heading);
761 
762  averagePoseCount++;
763  }
764  }
765 
766  Pose2 averagePose;
767  if (averagePoseCount > 0)
768  {
769  averagePosition /= averagePoseCount;
770 
771  thetaX /= averagePoseCount;
772  thetaY /= averagePoseCount;
773 
774  averagePose = Pose2(averagePosition, atan2(thetaY, thetaX));
775  }
776  else
777  {
778  throw std::runtime_error("Mapper FATAL ERROR - Unable to find best position");
779  }
780 
781  // delete pose response array
782  delete [] m_pPoseResponse;
783 
784 #ifdef KARTO_DEBUG
785  std::cout << "bestPose: " << averagePose << std::endl;
786  std::cout << "bestResponse: " << bestResponse << std::endl;
787 #endif
788 
789  if (!doingFineMatch)
790  {
791  ComputePositionalCovariance(averagePose, bestResponse, rSearchCenter, rSearchSpaceOffset,
792  rSearchSpaceResolution, searchAngleResolution, rCovariance);
793  }
794  else
795  {
796  ComputeAngularCovariance(averagePose, bestResponse, rSearchCenter,
797  searchAngleOffset, searchAngleResolution, rCovariance);
798  }
799 
800  rMean = averagePose;
801 
802 #ifdef KARTO_DEBUG
803  std::cout << "bestPose: " << averagePose << std::endl;
804 #endif
805 
806  if (bestResponse > 1.0)
807  {
808  bestResponse = 1.0;
809  }
810 
811  assert(math::InRange(bestResponse, 0.0, 1.0));
812  assert(math::InRange(rMean.GetHeading(), -KT_PI, KT_PI));
813 
814  return bestResponse;
815  }
816 
827  void ScanMatcher::ComputePositionalCovariance(const Pose2& rBestPose, kt_double bestResponse,
828  const Pose2& rSearchCenter,
829  const Vector2<kt_double>& rSearchSpaceOffset,
830  const Vector2<kt_double>& rSearchSpaceResolution,
831  kt_double searchAngleResolution, Matrix3& rCovariance)
832  {
833  // reset covariance to identity matrix
834  rCovariance.SetToIdentity();
835 
836  // if best response is vary small return max variance
837  if (bestResponse < KT_TOLERANCE)
838  {
839  rCovariance(0, 0) = MAX_VARIANCE; // XX
840  rCovariance(1, 1) = MAX_VARIANCE; // YY
841  rCovariance(2, 2) = 4 * math::Square(searchAngleResolution); // TH*TH
842 
843  return;
844  }
845 
846  kt_double accumulatedVarianceXX = 0;
847  kt_double accumulatedVarianceXY = 0;
848  kt_double accumulatedVarianceYY = 0;
849  kt_double norm = 0;
850 
851  kt_double dx = rBestPose.GetX() - rSearchCenter.GetX();
852  kt_double dy = rBestPose.GetY() - rSearchCenter.GetY();
853 
854  kt_double offsetX = rSearchSpaceOffset.GetX();
855  kt_double offsetY = rSearchSpaceOffset.GetY();
856 
857  kt_int32u nX = static_cast<kt_int32u>(math::Round(offsetX * 2.0 / rSearchSpaceResolution.GetX()) + 1);
858  kt_double startX = -offsetX;
859  assert(math::DoubleEqual(startX + (nX - 1) * rSearchSpaceResolution.GetX(), -startX));
860 
861  kt_int32u nY = static_cast<kt_int32u>(math::Round(offsetY * 2.0 / rSearchSpaceResolution.GetY()) + 1);
862  kt_double startY = -offsetY;
863  assert(math::DoubleEqual(startY + (nY - 1) * rSearchSpaceResolution.GetY(), -startY));
864 
865  for (kt_int32u yIndex = 0; yIndex < nY; yIndex++)
866  {
867  kt_double y = startY + yIndex * rSearchSpaceResolution.GetY();
868 
869  for (kt_int32u xIndex = 0; xIndex < nX; xIndex++)
870  {
871  kt_double x = startX + xIndex * rSearchSpaceResolution.GetX();
872 
873  Vector2<kt_int32s> gridPoint = m_pSearchSpaceProbs->WorldToGrid(Vector2<kt_double>(rSearchCenter.GetX() + x,
874  rSearchCenter.GetY() + y));
875  kt_double response = *(m_pSearchSpaceProbs->GetDataPointer(gridPoint));
876 
877  // response is not a low response
878  if (response >= (bestResponse - 0.1))
879  {
880  norm += response;
881  accumulatedVarianceXX += (math::Square(x - dx) * response);
882  accumulatedVarianceXY += ((x - dx) * (y - dy) * response);
883  accumulatedVarianceYY += (math::Square(y - dy) * response);
884  }
885  }
886  }
887 
888  if (norm > KT_TOLERANCE)
889  {
890  kt_double varianceXX = accumulatedVarianceXX / norm;
891  kt_double varianceXY = accumulatedVarianceXY / norm;
892  kt_double varianceYY = accumulatedVarianceYY / norm;
893  kt_double varianceTHTH = 4 * math::Square(searchAngleResolution);
894 
895  // lower-bound variances so that they are not too small;
896  // ensures that links are not too tight
897  kt_double minVarianceXX = 0.1 * math::Square(rSearchSpaceResolution.GetX());
898  kt_double minVarianceYY = 0.1 * math::Square(rSearchSpaceResolution.GetY());
899  varianceXX = math::Maximum(varianceXX, minVarianceXX);
900  varianceYY = math::Maximum(varianceYY, minVarianceYY);
901 
902  // increase variance for poorer responses
903  kt_double multiplier = 1.0 / bestResponse;
904  rCovariance(0, 0) = varianceXX * multiplier;
905  rCovariance(0, 1) = varianceXY * multiplier;
906  rCovariance(1, 0) = varianceXY * multiplier;
907  rCovariance(1, 1) = varianceYY * multiplier;
908  rCovariance(2, 2) = varianceTHTH; // this value will be set in ComputeAngularCovariance
909  }
910 
911  // if values are 0, set to MAX_VARIANCE
912  // values might be 0 if points are too sparse and thus don't hit other points
913  if (math::DoubleEqual(rCovariance(0, 0), 0.0))
914  {
915  rCovariance(0, 0) = MAX_VARIANCE;
916  }
917 
918  if (math::DoubleEqual(rCovariance(1, 1), 0.0))
919  {
920  rCovariance(1, 1) = MAX_VARIANCE;
921  }
922  }
923 
934  kt_double bestResponse,
935  const Pose2& rSearchCenter,
936  kt_double searchAngleOffset,
937  kt_double searchAngleResolution,
938  Matrix3& rCovariance)
939  {
940  // NOTE: do not reset covariance matrix
941 
942  // normalize angle difference
943  kt_double bestAngle = math::NormalizeAngleDifference(rBestPose.GetHeading(), rSearchCenter.GetHeading());
944 
945  Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(rBestPose.GetPosition());
946  kt_int32s gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);
947 
948  kt_int32u nAngles = static_cast<kt_int32u>(math::Round(searchAngleOffset * 2 / searchAngleResolution) + 1);
949 
950  kt_double angle = 0.0;
951  kt_double startAngle = rSearchCenter.GetHeading() - searchAngleOffset;
952 
953  kt_double norm = 0.0;
954  kt_double accumulatedVarianceThTh = 0.0;
955  for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
956  {
957  angle = startAngle + angleIndex * searchAngleResolution;
958  kt_double response = GetResponse(angleIndex, gridIndex);
959 
960  // response is not a low response
961  if (response >= (bestResponse - 0.1))
962  {
963  norm += response;
964  accumulatedVarianceThTh += (math::Square(angle - bestAngle) * response);
965  }
966  }
967  assert(math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset));
968 
969  if (norm > KT_TOLERANCE)
970  {
971  if (accumulatedVarianceThTh < KT_TOLERANCE)
972  {
973  accumulatedVarianceThTh = math::Square(searchAngleResolution);
974  }
975 
976  accumulatedVarianceThTh /= norm;
977  }
978  else
979  {
980  accumulatedVarianceThTh = 1000 * math::Square(searchAngleResolution);
981  }
982 
983  rCovariance(2, 2) = accumulatedVarianceThTh;
984  }
985 
992  {
993  m_pCorrelationGrid->Clear();
994 
995  // add all scans to grid
997  {
998  if (*iter == NULL)
999  {
1000  continue;
1001  }
1002 
1003  AddScan(*iter, viewPoint);
1004  }
1005  }
1006 
1013  {
1014  m_pCorrelationGrid->Clear();
1015 
1016  // add all scans to grid
1018  {
1019  if (iter->second == NULL)
1020  {
1021  continue;
1022  }
1023 
1024  AddScan(iter->second, viewPoint);
1025  }
1026  }
1027 
1034  void ScanMatcher::AddScan(LocalizedRangeScan* pScan, const Vector2<kt_double>& rViewPoint, kt_bool doSmear)
1035  {
1036  PointVectorDouble validPoints = FindValidPoints(pScan, rViewPoint);
1037 
1038  // put in all valid points
1039  const_forEach(PointVectorDouble, &validPoints)
1040  {
1041  Vector2<kt_int32s> gridPoint = m_pCorrelationGrid->WorldToGrid(*iter);
1042  if (!math::IsUpTo(gridPoint.GetX(), m_pCorrelationGrid->GetROI().GetWidth()) ||
1043  !math::IsUpTo(gridPoint.GetY(), m_pCorrelationGrid->GetROI().GetHeight()))
1044  {
1045  // point not in grid
1046  continue;
1047  }
1048 
1049  int gridIndex = m_pCorrelationGrid->GridIndex(gridPoint);
1050 
1051  // set grid cell as occupied
1052  if (m_pCorrelationGrid->GetDataPointer()[gridIndex] == GridStates_Occupied)
1053  {
1054  // value already set
1055  continue;
1056  }
1057 
1058  m_pCorrelationGrid->GetDataPointer()[gridIndex] = GridStates_Occupied;
1059 
1060  // smear grid
1061  if (doSmear == true)
1062  {
1063  m_pCorrelationGrid->SmearPoint(gridPoint);
1064  }
1065  }
1066  }
1067 
1075  {
1076  const PointVectorDouble& rPointReadings = pScan->GetPointReadings();
1077 
1078  // points must be at least 10 cm away when making comparisons of inside/outside of viewpoint
1079  const kt_double minSquareDistance = math::Square(0.1); // in m^2
1080 
1081  // this iterator lags from the main iterator adding points only when the points are on
1082  // the same side as the viewpoint
1083  PointVectorDouble::const_iterator trailingPointIter = rPointReadings.begin();
1084  PointVectorDouble validPoints;
1085 
1086  Vector2<kt_double> firstPoint;
1087  kt_bool firstTime = true;
1088  const_forEach(PointVectorDouble, &rPointReadings)
1089  {
1090  Vector2<kt_double> currentPoint = *iter;
1091 
1092  if (firstTime && !std::isnan(currentPoint.GetX()) && !std::isnan(currentPoint.GetY()))
1093  {
1094  firstPoint = currentPoint;
1095  firstTime = false;
1096  }
1097 
1098  Vector2<kt_double> delta = firstPoint - currentPoint;
1099  if (delta.SquaredLength() > minSquareDistance)
1100  {
1101  // This compute the Determinant (viewPoint FirstPoint, viewPoint currentPoint)
1102  // Which computes the direction of rotation, if the rotation is counterclock
1103  // wise then we are looking at data we should keep. If it's negative rotation
1104  // we should not included in in the matching
1105  // have enough distance, check viewpoint
1106  double a = rViewPoint.GetY() - firstPoint.GetY();
1107  double b = firstPoint.GetX() - rViewPoint.GetX();
1108  double c = firstPoint.GetY() * rViewPoint.GetX() - firstPoint.GetX() * rViewPoint.GetY();
1109  double ss = currentPoint.GetX() * a + currentPoint.GetY() * b + c;
1110 
1111  // reset beginning point
1112  firstPoint = currentPoint;
1113 
1114  if (ss < 0.0) // wrong side, skip and keep going
1115  {
1116  trailingPointIter = iter;
1117  }
1118  else
1119  {
1120  for (; trailingPointIter != iter; ++trailingPointIter)
1121  {
1122  validPoints.push_back(*trailingPointIter);
1123  }
1124  }
1125  }
1126  }
1127 
1128  return validPoints;
1129  }
1130 
1137  kt_double ScanMatcher::GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const
1138  {
1139  kt_double response = 0.0;
1140 
1141  // add up value for each point
1142  kt_int8u* pByte = m_pCorrelationGrid->GetDataPointer() + gridPositionIndex;
1143 
1144  const LookupArray* pOffsets = m_pGridLookup->GetLookupArray(angleIndex);
1145  assert(pOffsets != NULL);
1146 
1147  // get number of points in offset list
1148  kt_int32u nPoints = pOffsets->GetSize();
1149  if (nPoints == 0)
1150  {
1151  return response;
1152  }
1153 
1154  // calculate response
1155  kt_int32s* pAngleIndexPointer = pOffsets->GetArrayPointer();
1156  for (kt_int32u i = 0; i < nPoints; i++)
1157  {
1158  // ignore points that fall off the grid
1159  kt_int32s pointGridIndex = gridPositionIndex + pAngleIndexPointer[i];
1160  if (!math::IsUpTo(pointGridIndex, m_pCorrelationGrid->GetDataSize()) || pAngleIndexPointer[i] == INVALID_SCAN)
1161  {
1162  continue;
1163  }
1164 
1165  // uses index offsets to efficiently find location of point in the grid
1166  response += pByte[pAngleIndexPointer[i]];
1167  }
1168 
1169  // normalize response
1170  response /= (nPoints * GridStates_Occupied);
1171  assert(fabs(response) <= 1.0);
1172 
1173  return response;
1174  }
1175 
1176 
1180 
1181  template<typename T>
1183  {
1184  public:
1189  {
1190  }
1192  : GraphTraversal<T>(pGraph)
1193  {
1194  }
1195 
1200  {
1201  }
1202 
1203  public:
1210  virtual std::vector<T*> TraverseForScans(Vertex<T>* pStartVertex, Visitor<T>* pVisitor)
1211  {
1212  std::vector<Vertex<T>*> validVertices = TraverseForVertices(pStartVertex, pVisitor);
1213 
1214  std::vector<T*> objects;
1215  forEach(typename std::vector<Vertex<T>*>, &validVertices)
1216  {
1217  objects.push_back((*iter)->GetObject());
1218  }
1219 
1220  return objects;
1221  }
1222 
1229  virtual std::vector<Vertex<T>*> TraverseForVertices(Vertex<T>* pStartVertex, Visitor<T>* pVisitor)
1230  {
1231  std::queue<Vertex<T>*> toVisit;
1232  std::set<Vertex<T>*> seenVertices;
1233  std::vector<Vertex<T>*> validVertices;
1234 
1235  toVisit.push(pStartVertex);
1236  seenVertices.insert(pStartVertex);
1237 
1238  do
1239  {
1240  Vertex<T>* pNext = toVisit.front();
1241  toVisit.pop();
1242 
1243  if (pNext != NULL && pVisitor->Visit(pNext))
1244  {
1245  // vertex is valid, explore neighbors
1246  validVertices.push_back(pNext);
1247 
1248  std::vector<Vertex<T>*> adjacentVertices = pNext->GetAdjacentVertices();
1249  forEach(typename std::vector<Vertex<T>*>, &adjacentVertices)
1250  {
1251  Vertex<T>* pAdjacent = *iter;
1252 
1253  // adjacent vertex has not yet been seen, add to queue for processing
1254  if (seenVertices.find(pAdjacent) == seenVertices.end())
1255  {
1256  toVisit.push(pAdjacent);
1257  seenVertices.insert(pAdjacent);
1258  }
1259  }
1260  }
1261  } while (toVisit.empty() == false);
1262 
1263  return validVertices;
1264  }
1265 
1266  friend class boost::serialization::access;
1267  template<class Archive>
1268  void serialize(Archive &ar, const unsigned int version)
1269  {
1270  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GraphTraversal<T>);
1271  }
1272  }; // class BreadthFirstTraversal
1273 
1277 
1278  class NearScanVisitor : public Visitor<LocalizedRangeScan>
1279  {
1280  public:
1281  NearScanVisitor(LocalizedRangeScan* pScan, kt_double maxDistance, kt_bool useScanBarycenter)
1282  : m_MaxDistanceSquared(math::Square(maxDistance))
1283  , m_UseScanBarycenter(useScanBarycenter)
1284  {
1285  m_CenterPose = pScan->GetReferencePose(m_UseScanBarycenter);
1286  }
1287 
1289  {
1290  try
1291  {
1292  LocalizedRangeScan* pScan = pVertex->GetObject();
1293  Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter);
1294  kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition());
1295  return (squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE);
1296  }
1297  catch(...)
1298  {
1299  // relocalization vertex elements missing
1300  std::cout << "Unable to visit valid vertex elements!" << std::endl;
1301  return false;
1302  }
1303  }
1304 
1305  protected:
1309  friend class boost::serialization::access;
1310  template<class Archive>
1311  void serialize(Archive &ar, const unsigned int version)
1312  {
1313  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Visitor<LocalizedRangeScan>);
1314  ar & BOOST_SERIALIZATION_NVP(m_CenterPose);
1315  ar & BOOST_SERIALIZATION_NVP(m_MaxDistanceSquared);
1316  ar & BOOST_SERIALIZATION_NVP(m_UseScanBarycenter);
1317  }
1318  }; // NearScanVisitor
1319 
1323 
1324  class NearPoseVisitor : public Visitor<LocalizedRangeScan>
1325  {
1326  public:
1327  NearPoseVisitor(Pose2 refPose, kt_double maxDistance, kt_bool useScanBarycenter)
1328  : m_MaxDistanceSquared(math::Square(maxDistance))
1329  , m_UseScanBarycenter(useScanBarycenter)
1330  {
1331  m_CenterPose = refPose;
1332  }
1333 
1335  {
1336  LocalizedRangeScan* pScan = pVertex->GetObject();
1337 
1338  Pose2 pose = pScan->GetReferencePose(m_UseScanBarycenter);
1339 
1340  kt_double squaredDistance = pose.GetPosition().SquaredDistance(m_CenterPose.GetPosition());
1341  return (squaredDistance <= m_MaxDistanceSquared - KT_TOLERANCE);
1342  }
1343 
1344  protected:
1348  friend class boost::serialization::access;
1349  template<class Archive>
1350  void serialize(Archive &ar, const unsigned int version)
1351  {
1352  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Visitor<LocalizedRangeScan>);
1353  ar & BOOST_SERIALIZATION_NVP(m_CenterPose);
1354  ar & BOOST_SERIALIZATION_NVP(m_MaxDistanceSquared);
1355  ar & BOOST_SERIALIZATION_NVP(m_UseScanBarycenter);
1356  }
1357  }; // NearPoseVisitor
1358 
1362 
1363 
1364  MapperGraph::MapperGraph(Mapper* pMapper, kt_double rangeThreshold)
1365  : m_pMapper(pMapper)
1366  {
1370  assert(m_pLoopScanMatcher);
1371 
1373  }
1374 
1376  {
1377  if (m_pLoopScanMatcher)
1378  {
1379  delete m_pLoopScanMatcher;
1381  }
1382  if (m_pTraversal)
1383  {
1384  delete m_pTraversal;
1385  m_pTraversal = NULL;
1386  }
1387  }
1388 
1390  {
1391  assert(pScan);
1392 
1393  if (pScan != NULL)
1394  {
1398  {
1399  m_pMapper->m_pScanOptimizer->AddNode(pVertex);
1400  }
1401  return pVertex;
1402  }
1403  }
1404 
1405  void MapperGraph::AddEdges(LocalizedRangeScan* pScan, const Matrix3& rCovariance)
1406  {
1408 
1409  const Name rSensorName = pScan->GetSensorName();
1410 
1411  // link to previous scan
1412  kt_int32s previousScanNum = pScan->GetStateId() - 1;
1413  if (pSensorManager->GetLastScan(rSensorName) != NULL)
1414  {
1415  assert(previousScanNum >= 0);
1416  LocalizedRangeScan* pPrevScan = pSensorManager->GetScan(rSensorName, previousScanNum);
1417  if (!pPrevScan)
1418  {
1419  return;
1420  }
1421  LinkScans(pPrevScan, pScan, pScan->GetSensorPose(), rCovariance);
1422  }
1423 
1424  Pose2Vector means;
1425  std::vector<Matrix3> covariances;
1426 
1427  // first scan (link to first scan of other robots)
1428  if (pSensorManager->GetLastScan(rSensorName) == NULL)
1429  {
1430  assert(pSensorManager->GetScans(rSensorName).size() == 1);
1431 
1432  std::vector<Name> deviceNames = pSensorManager->GetSensorNames();
1433  forEach(std::vector<Name>, &deviceNames)
1434  {
1435  const Name& rCandidateSensorName = *iter;
1436 
1437  // skip if candidate device is the same or other device has no scans
1438  if ((rCandidateSensorName == rSensorName) || (pSensorManager->GetScans(rCandidateSensorName).empty()))
1439  {
1440  continue;
1441  }
1442 
1443  Pose2 bestPose;
1444  Matrix3 covariance;
1446  pSensorManager->GetScans(rCandidateSensorName),
1447  bestPose, covariance);
1448  LinkScans(pScan, pSensorManager->GetScan(rCandidateSensorName, 0), bestPose, covariance);
1449 
1450  // only add to means and covariances if response was high "enough"
1452  {
1453  means.push_back(bestPose);
1454  covariances.push_back(covariance);
1455  }
1456  }
1457  }
1458  else
1459  {
1460  // link to running scans
1461  Pose2 scanPose = pScan->GetSensorPose();
1462  means.push_back(scanPose);
1463  covariances.push_back(rCovariance);
1464  LinkChainToScan(pSensorManager->GetRunningScans(rSensorName), pScan, scanPose, rCovariance);
1465  }
1466 
1467  // link to other near chains (chains that include new scan are invalid)
1468  LinkNearChains(pScan, means, covariances);
1469 
1470  if (!means.empty())
1471  {
1472  pScan->SetSensorPose(ComputeWeightedMean(means, covariances));
1473  }
1474  }
1475 
1477  {
1478  kt_bool loopClosed = false;
1479 
1480  kt_int32u scanIndex = 0;
1481 
1482  LocalizedRangeScanVector candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
1483 
1484  while (!candidateChain.empty())
1485  {
1486  Pose2 bestPose;
1487  Matrix3 covariance;
1488  kt_double coarseResponse = m_pLoopScanMatcher->MatchScan(pScan, candidateChain,
1489  bestPose, covariance, false, false);
1490 
1491  std::stringstream stream;
1492  stream << "COARSE RESPONSE: " << coarseResponse
1493  << " (> " << m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue() << ")"
1494  << std::endl;
1495  stream << " var: " << covariance(0, 0) << ", " << covariance(1, 1)
1496  << " (< " << m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue() << ")";
1497 
1498  m_pMapper->FireLoopClosureCheck(stream.str());
1499 
1500  if ((coarseResponse > m_pMapper->m_pLoopMatchMinimumResponseCoarse->GetValue()) &&
1501  (covariance(0, 0) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()) &&
1502  (covariance(1, 1) < m_pMapper->m_pLoopMatchMaximumVarianceCoarse->GetValue()))
1503  {
1504  LocalizedRangeScan tmpScan(pScan->GetSensorName(), pScan->GetRangeReadingsVector());
1505  tmpScan.SetUniqueId(pScan->GetUniqueId());
1506  tmpScan.SetTime(pScan->GetTime());
1507  tmpScan.SetStateId(pScan->GetStateId());
1508  tmpScan.SetCorrectedPose(pScan->GetCorrectedPose());
1509  tmpScan.SetSensorPose(bestPose); // This also updates OdometricPose.
1510  kt_double fineResponse = m_pMapper->m_pSequentialScanMatcher->MatchScan(&tmpScan, candidateChain,
1511  bestPose, covariance, false);
1512 
1513  std::stringstream stream1;
1514  stream1 << "FINE RESPONSE: " << fineResponse << " (>"
1515  << m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue() << ")" << std::endl;
1516  m_pMapper->FireLoopClosureCheck(stream1.str());
1517 
1518  if (fineResponse < m_pMapper->m_pLoopMatchMinimumResponseFine->GetValue())
1519  {
1520  m_pMapper->FireLoopClosureCheck("REJECTED!");
1521  }
1522  else
1523  {
1524  m_pMapper->FireBeginLoopClosure("Closing loop...");
1525 
1526  pScan->SetSensorPose(bestPose);
1527  LinkChainToScan(candidateChain, pScan, bestPose, covariance);
1528  CorrectPoses();
1529 
1530  m_pMapper->FireEndLoopClosure("Loop closed!");
1531 
1532  loopClosed = true;
1533  }
1534  }
1535 
1536  candidateChain = FindPossibleLoopClosure(pScan, rSensorName, scanIndex);
1537  }
1538 
1539  return loopClosed;
1540  }
1541 
1543  const Pose2& rPose) const
1544  {
1545  LocalizedRangeScan* pClosestScan = NULL;
1546  kt_double bestSquaredDistance = DBL_MAX;
1547 
1549  {
1550  Pose2 scanPose = (*iter)->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
1551 
1552  kt_double squaredDistance = rPose.GetPosition().SquaredDistance(scanPose.GetPosition());
1553  if (squaredDistance < bestSquaredDistance)
1554  {
1555  bestSquaredDistance = squaredDistance;
1556  pClosestScan = *iter;
1557  }
1558  }
1559 
1560  return pClosestScan;
1561  }
1562 
1564  LocalizedRangeScan* pTargetScan, kt_bool& rIsNewEdge)
1565  {
1566  std::map<int, Vertex<LocalizedRangeScan>* >::iterator v1 = m_Vertices[pSourceScan->GetSensorName()].find(pSourceScan->GetStateId());
1567  std::map<int, Vertex<LocalizedRangeScan>* >::iterator v2 = m_Vertices[pTargetScan->GetSensorName()].find(pTargetScan->GetStateId());
1568 
1569  if (v1 == m_Vertices[pSourceScan->GetSensorName()].end() ||
1570  v2 == m_Vertices[pSourceScan->GetSensorName()].end())
1571  {
1572  std::cout << "AddEdge: At least one vertex is invalid." << std::endl;
1573  return NULL;
1574  }
1575 
1576  // see if edge already exists
1577  const_forEach(std::vector<Edge<LocalizedRangeScan>*>, &(v1->second->GetEdges()))
1578  {
1579  Edge<LocalizedRangeScan>* pEdge = *iter;
1580 
1581  if (pEdge->GetTarget() == v2->second)
1582  {
1583  rIsNewEdge = false;
1584  return pEdge;
1585  }
1586  }
1587 
1588  Edge<LocalizedRangeScan>* pEdge = new Edge<LocalizedRangeScan>(v1->second, v2->second);
1590  rIsNewEdge = true;
1591  return pEdge;
1592  }
1593 
1595  const Pose2& rMean, const Matrix3& rCovariance)
1596  {
1597  kt_bool isNewEdge = true;
1598  Edge<LocalizedRangeScan>* pEdge = AddEdge(pFromScan, pToScan, isNewEdge);
1599 
1600  if (pEdge == NULL)
1601  {
1602  return;
1603  }
1604 
1605  // only attach link information if the edge is new
1606  if (isNewEdge == true)
1607  {
1608  pEdge->SetLabel(new LinkInfo(pFromScan->GetSensorPose(), rMean, rCovariance));
1610  {
1612  }
1613  }
1614  }
1615 
1616  void MapperGraph::LinkNearChains(LocalizedRangeScan* pScan, Pose2Vector& rMeans, std::vector<Matrix3>& rCovariances)
1617  {
1618  const std::vector<LocalizedRangeScanVector> nearChains = FindNearChains(pScan);
1619  const_forEach(std::vector<LocalizedRangeScanVector>, &nearChains)
1620  {
1621  if (iter->size() < m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
1622  {
1623  continue;
1624  }
1625 
1626  Pose2 mean;
1627  Matrix3 covariance;
1628  // match scan against "near" chain
1629  kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, *iter, mean, covariance, false);
1631  {
1632  rMeans.push_back(mean);
1633  rCovariances.push_back(covariance);
1634  LinkChainToScan(*iter, pScan, mean, covariance);
1635  }
1636  }
1637  }
1638 
1640  const Pose2& rMean, const Matrix3& rCovariance)
1641  {
1643 
1644  LocalizedRangeScan* pClosestScan = GetClosestScanToPose(rChain, pose);
1645  assert(pClosestScan != NULL);
1646 
1647  Pose2 closestScanPose = pClosestScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
1648 
1649  kt_double squaredDistance = pose.GetPosition().SquaredDistance(closestScanPose.GetPosition());
1651  {
1652  LinkScans(pClosestScan, pScan, rMean, rCovariance);
1653  }
1654  }
1655 
1656  std::vector<LocalizedRangeScanVector> MapperGraph::FindNearChains(LocalizedRangeScan* pScan)
1657  {
1658  std::vector<LocalizedRangeScanVector> nearChains;
1659 
1661 
1662  // to keep track of which scans have been added to a chain
1663  LocalizedRangeScanVector processed;
1664 
1665  const LocalizedRangeScanVector nearLinkedScans = FindNearLinkedScans(pScan,
1667  const_forEach(LocalizedRangeScanVector, &nearLinkedScans)
1668  {
1669  LocalizedRangeScan* pNearScan = *iter;
1670 
1671  if (pNearScan == pScan)
1672  {
1673  continue;
1674  }
1675 
1676  // scan has already been processed, skip
1677  if (find(processed.begin(), processed.end(), pNearScan) != processed.end())
1678  {
1679  continue;
1680  }
1681 
1682  processed.push_back(pNearScan);
1683 
1684  // build up chain
1685  kt_bool isValidChain = true;
1686  std::list<LocalizedRangeScan*> chain;
1687 
1688  // add scans before current scan being processed
1689  for (kt_int32s candidateScanNum = pNearScan->GetStateId() - 1; candidateScanNum >= 0; candidateScanNum--)
1690  {
1691  LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(),
1692  candidateScanNum);
1693 
1694  // chain is invalid--contains scan being added
1695  if (pCandidateScan == pScan)
1696  {
1697  isValidChain = false;
1698  }
1699 
1700  // probably removed in localization mode
1701  if (pCandidateScan == NULL)
1702  {
1703  continue;
1704  }
1705 
1706  Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
1707  kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
1708 
1710  {
1711  chain.push_front(pCandidateScan);
1712  processed.push_back(pCandidateScan);
1713  }
1714  else
1715  {
1716  break;
1717  }
1718  }
1719 
1720  chain.push_back(pNearScan);
1721 
1722  // add scans after current scan being processed
1723  kt_int32u end = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(pNearScan->GetSensorName()).size());
1724  for (kt_int32u candidateScanNum = pNearScan->GetStateId() + 1; candidateScanNum < end; candidateScanNum++)
1725  {
1726  LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(pNearScan->GetSensorName(),
1727  candidateScanNum);
1728 
1729  if (pCandidateScan == pScan)
1730  {
1731  isValidChain = false;
1732  }
1733 
1734  // probably removed in localization mode
1735  if (pCandidateScan == NULL)
1736  {
1737  continue;
1738  }
1739 
1740  Pose2 candidatePose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());;
1741  kt_double squaredDistance = scanPose.GetPosition().SquaredDistance(candidatePose.GetPosition());
1742 
1744  {
1745  chain.push_back(pCandidateScan);
1746  processed.push_back(pCandidateScan);
1747  }
1748  else
1749  {
1750  break;
1751  }
1752  }
1753 
1754  if (isValidChain)
1755  {
1756  // change list to vector
1757  LocalizedRangeScanVector tempChain;
1758  std::copy(chain.begin(), chain.end(), std::inserter(tempChain, tempChain.begin()));
1759  // add chain to collection
1760  nearChains.push_back(tempChain);
1761  }
1762  }
1763 
1764  return nearChains;
1765  }
1766 
1768  {
1769  NearScanVisitor* pVisitor = new NearScanVisitor(pScan, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue());
1770  LocalizedRangeScanVector nearLinkedScans = m_pTraversal->TraverseForScans(GetVertex(pScan), pVisitor);
1771  delete pVisitor;
1772 
1773  return nearLinkedScans;
1774  }
1775 
1776  std::vector<Vertex<LocalizedRangeScan>*> MapperGraph::FindNearLinkedVertices(LocalizedRangeScan* pScan, kt_double maxDistance)
1777  {
1778  NearScanVisitor* pVisitor = new NearScanVisitor(pScan, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue());
1779  std::vector<Vertex<LocalizedRangeScan>*> nearLinkedVertices = m_pTraversal->TraverseForVertices(GetVertex(pScan), pVisitor);
1780  delete pVisitor;
1781 
1782  return nearLinkedVertices;
1783  }
1784 
1786  {
1787  NearPoseVisitor* pVisitor = new NearPoseVisitor(refPose, maxDistance, m_pMapper->m_pUseScanBarycenter->GetValue());
1788 
1789  Vertex<LocalizedRangeScan>* closestVertex = FindNearByScan(name, refPose);
1790 
1791  LocalizedRangeScanVector nearLinkedScans = m_pTraversal->TraverseForScans(closestVertex, pVisitor);
1792  delete pVisitor;
1793 
1794  return nearLinkedScans;
1795  }
1796 
1797  std::vector<Vertex<LocalizedRangeScan>*> MapperGraph::FindNearByVertices(Name name, const Pose2 refPose, kt_double maxDistance)
1798  {
1799  VertexMap vertexMap = GetVertices();
1800  std::map<int, Vertex<LocalizedRangeScan>*>& vertices = vertexMap[name];
1801 
1802  std::vector<Vertex<LocalizedRangeScan>*> vertices_to_search;
1803  std::map<int, Vertex<LocalizedRangeScan>*>::iterator it;
1804  for (it = vertices.begin(); it != vertices.end(); ++it)
1805  {
1806  if (it->second)
1807  {
1808  vertices_to_search.push_back(it->second);
1809  }
1810  }
1811 
1812  const size_t dim = 2;
1813 
1815  const P2KD p2kd(vertices_to_search);
1816 
1818 
1819  my_kd_tree_t index(dim, p2kd, nanoflann::KDTreeSingleIndexAdaptorParams(10) );
1820  index.buildIndex();
1821 
1822  std::vector<std::pair<size_t,double> > ret_matches;
1823  const double query_pt[2] = {refPose.GetX(), refPose.GetY()};
1824  nanoflann::SearchParams params;
1825  const size_t num_results = index.radiusSearch(&query_pt[0], maxDistance, ret_matches, params);
1826 
1827  std::vector<Vertex<LocalizedRangeScan>*> rtn_vertices;
1828  rtn_vertices.reserve(ret_matches.size());
1829  for (uint i = 0; i != ret_matches.size(); i++)
1830  {
1831  rtn_vertices.push_back(vertices_to_search[ret_matches[i].first]);
1832  }
1833  return rtn_vertices;
1834  }
1835 
1837  {
1838  VertexMap vertexMap = GetVertices();
1839  std::map<int, Vertex<LocalizedRangeScan>*>& vertices = vertexMap[name];
1840 
1841  std::vector<Vertex<LocalizedRangeScan>*> vertices_to_search;
1842  std::map<int, Vertex<LocalizedRangeScan>*>::iterator it;
1843  for (it = vertices.begin(); it != vertices.end(); ++it)
1844  {
1845  if (it->second)
1846  {
1847  vertices_to_search.push_back(it->second);
1848  }
1849  }
1850 
1851  size_t num_results = 1;
1852  const size_t dim = 2;
1853 
1855  const P2KD p2kd(vertices_to_search);
1856 
1858 
1859  my_kd_tree_t index(dim, p2kd, nanoflann::KDTreeSingleIndexAdaptorParams(10) );
1860  index.buildIndex();
1861 
1862  std::vector<size_t> ret_index(num_results);
1863  std::vector<double> out_dist_sqr(num_results);
1864  const double query_pt[2] = {refPose.GetX(), refPose.GetY()};
1865  num_results = index.knnSearch(&query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]);
1866 
1867  if (num_results > 0)
1868  {
1869  return vertices_to_search[ret_index[0]];
1870  }
1871  else
1872  {
1873  return NULL;
1874  }
1875  }
1876 
1877  Pose2 MapperGraph::ComputeWeightedMean(const Pose2Vector& rMeans, const std::vector<Matrix3>& rCovariances) const
1878  {
1879  assert(rMeans.size() == rCovariances.size());
1880 
1881  // compute sum of inverses and create inverse list
1882  std::vector<Matrix3> inverses;
1883  inverses.reserve(rCovariances.size());
1884 
1885  Matrix3 sumOfInverses;
1886  const_forEach(std::vector<Matrix3>, &rCovariances)
1887  {
1888  Matrix3 inverse = iter->Inverse();
1889  inverses.push_back(inverse);
1890 
1891  sumOfInverses += inverse;
1892  }
1893  Matrix3 inverseOfSumOfInverses = sumOfInverses.Inverse();
1894 
1895  // compute weighted mean
1896  Pose2 accumulatedPose;
1897  kt_double thetaX = 0.0;
1898  kt_double thetaY = 0.0;
1899 
1900  Pose2Vector::const_iterator meansIter = rMeans.begin();
1901  const_forEach(std::vector<Matrix3>, &inverses)
1902  {
1903  Pose2 pose = *meansIter;
1904  kt_double angle = pose.GetHeading();
1905  thetaX += cos(angle);
1906  thetaY += sin(angle);
1907 
1908  Matrix3 weight = inverseOfSumOfInverses * (*iter);
1909  accumulatedPose += weight * pose;
1910 
1911  ++meansIter;
1912  }
1913 
1914  thetaX /= rMeans.size();
1915  thetaY /= rMeans.size();
1916  accumulatedPose.SetHeading(atan2(thetaY, thetaX));
1917 
1918  return accumulatedPose;
1919  }
1920 
1922  const Name& rSensorName,
1923  kt_int32u& rStartNum)
1924  {
1925  LocalizedRangeScanVector chain; // return value
1926 
1928 
1929  // possible loop closure chain should not include close scans that have a
1930  // path of links to the scan of interest
1931  const LocalizedRangeScanVector nearLinkedScans =
1933 
1934  kt_int32u nScans = static_cast<kt_int32u>(m_pMapper->m_pMapperSensorManager->GetScans(rSensorName).size());
1935  for (; rStartNum < nScans; rStartNum++)
1936  {
1937  LocalizedRangeScan* pCandidateScan = m_pMapper->m_pMapperSensorManager->GetScan(rSensorName, rStartNum);
1938 
1939  if (pCandidateScan == NULL)
1940  {
1941  continue;
1942  }
1943 
1944  Pose2 candidateScanPose = pCandidateScan->GetReferencePose(m_pMapper->m_pUseScanBarycenter->GetValue());
1945 
1946  kt_double squaredDistance = candidateScanPose.GetPosition().SquaredDistance(pose.GetPosition());
1948  {
1949  // a linked scan cannot be in the chain
1950  if (find(nearLinkedScans.begin(), nearLinkedScans.end(), pCandidateScan) != nearLinkedScans.end())
1951  {
1952  chain.clear();
1953  }
1954  else
1955  {
1956  chain.push_back(pCandidateScan);
1957  }
1958  }
1959  else
1960  {
1961  // return chain if it is long "enough"
1962  if (chain.size() >= m_pMapper->m_pLoopMatchMinimumChainSize->GetValue())
1963  {
1964  return chain;
1965  }
1966  else
1967  {
1968  chain.clear();
1969  }
1970  }
1971  }
1972 
1973  return chain;
1974  }
1975 
1977  {
1978  // optimize scans!
1980  if (pSolver != NULL)
1981  {
1982  pSolver->Compute();
1983 
1985  {
1987  if (scan == NULL)
1988  {
1989  continue;
1990  }
1991  scan->SetSensorPose(iter->second);
1992  }
1993 
1994  pSolver->Clear();
1995  }
1996  }
1997 
2001 
2006  : Module("Mapper")
2007  , m_Initialized(false)
2008  , m_pSequentialScanMatcher(NULL)
2009  , m_pMapperSensorManager(NULL)
2010  , m_pGraph(NULL)
2011  , m_pScanOptimizer(NULL)
2012  {
2014  }
2015 
2019  Mapper::Mapper(const std::string& rName)
2020  : Module(rName)
2021  , m_Initialized(false)
2024  , m_pGraph(NULL)
2026  {
2028  }
2029 
2034  {
2035  Reset();
2036 
2037  delete m_pMapperSensorManager;
2038  }
2039 
2041  {
2043  "UseScanMatching",
2044  "When set to true, the mapper will use a scan matching algorithm. "
2045  "In most real-world situations this should be set to true so that the "
2046  "mapper algorithm can correct for noise and errors in odometry and "
2047  "scan data. In some simulator environments where the simulated scan "
2048  "and odometry data are very accurate, the scan matching algorithm can "
2049  "produce worse results. In those cases set this to false to improve "
2050  "results.",
2051  true,
2053 
2055  "UseScanBarycenter",
2056  "Use the barycenter of scan endpoints to define distances between "
2057  "scans.",
2058  true, GetParameterManager());
2059 
2061  "MinimumTimeInterval",
2062  "Sets the minimum time between scans. If a new scan's time stamp is "
2063  "longer than MinimumTimeInterval from the previously processed scan, "
2064  "the mapper will use the data from the new scan. Otherwise, it will "
2065  "discard the new scan if it also does not meet the minimum travel "
2066  "distance and heading requirements. For performance reasons, it is "
2067  "generally it is a good idea to only process scans if a reasonable "
2068  "amount of time has passed. This parameter is particularly useful "
2069  "when there is a need to process scans while the robot is stationary.",
2070  3600, GetParameterManager());
2071 
2073  "MinimumTravelDistance",
2074  "Sets the minimum travel between scans. If a new scan's position is "
2075  "more than minimumTravelDistance from the previous scan, the mapper "
2076  "will use the data from the new scan. Otherwise, it will discard the "
2077  "new scan if it also does not meet the minimum change in heading "
2078  "requirement. For performance reasons, generally it is a good idea to "
2079  "only process scans if the robot has moved a reasonable amount.",
2080  0.2, GetParameterManager());
2081 
2083  "MinimumTravelHeading",
2084  "Sets the minimum heading change between scans. If a new scan's "
2085  "heading is more than MinimumTravelHeading from the previous scan, the "
2086  "mapper will use the data from the new scan. Otherwise, it will "
2087  "discard the new scan if it also does not meet the minimum travel "
2088  "distance requirement. For performance reasons, generally it is a good "
2089  "idea to only process scans if the robot has moved a reasonable "
2090  "amount.",
2092 
2094  "ScanBufferSize",
2095  "Scan buffer size is the length of the scan chain stored for scan "
2096  "matching. \"ScanBufferSize\" should be set to approximately "
2097  "\"ScanBufferMaximumScanDistance\" / \"MinimumTravelDistance\". The "
2098  "idea is to get an area approximately 20 meters long for scan "
2099  "matching. For example, if we add scans every MinimumTravelDistance == "
2100  "0.3 meters, then \"scanBufferSize\" should be 20 / 0.3 = 67.)",
2101  70, GetParameterManager());
2102 
2104  "ScanBufferMaximumScanDistance",
2105  "Scan buffer maximum scan distance is the maximum distance between the "
2106  "first and last scans in the scan chain stored for matching.",
2107  20.0, GetParameterManager());
2108 
2110  "LinkMatchMinimumResponseFine",
2111  "Scans are linked only if the correlation response value is greater "
2112  "than this value.",
2113  0.8, GetParameterManager());
2114 
2116  "LinkScanMaximumDistance",
2117  "Maximum distance between linked scans. Scans that are farther apart "
2118  "will not be linked regardless of the correlation response value.",
2119  10.0, GetParameterManager());
2120 
2122  "LoopSearchMaximumDistance",
2123  "Scans less than this distance from the current position will be "
2124  "considered for a match in loop closure.",
2125  4.0, GetParameterManager());
2126 
2128  "DoLoopClosing",
2129  "Enable/disable loop closure.",
2130  true, GetParameterManager());
2131 
2133  "LoopMatchMinimumChainSize",
2134  "When the loop closure detection finds a candidate it must be part of "
2135  "a large set of linked scans. If the chain of scans is less than this "
2136  "value we do not attempt to close the loop.",
2137  10, GetParameterManager());
2138 
2140  "LoopMatchMaximumVarianceCoarse",
2141  "The co-variance values for a possible loop closure have to be less "
2142  "than this value to consider a viable solution. This applies to the "
2143  "coarse search.",
2145 
2147  "LoopMatchMinimumResponseCoarse",
2148  "If response is larger then this, then initiate loop closure search at "
2149  "the coarse resolution.",
2150  0.8, GetParameterManager());
2151 
2153  "LoopMatchMinimumResponseFine",
2154  "If response is larger then this, then initiate loop closure search at "
2155  "the fine resolution.",
2156  0.8, GetParameterManager());
2157 
2159  // CorrelationParameters correlationParameters;
2160 
2162  "CorrelationSearchSpaceDimension",
2163  "The size of the search grid used by the matcher. The search grid will "
2164  "have the size CorrelationSearchSpaceDimension * "
2165  "CorrelationSearchSpaceDimension",
2166  0.3, GetParameterManager());
2167 
2169  "CorrelationSearchSpaceResolution",
2170  "The resolution (size of a grid cell) of the correlation grid.",
2171  0.01, GetParameterManager());
2172 
2174  "CorrelationSearchSpaceSmearDeviation",
2175  "The point readings are smeared by this value in X and Y to create a "
2176  "smoother response.",
2177  0.03, GetParameterManager());
2178 
2179 
2181  // CorrelationParameters loopCorrelationParameters;
2182 
2184  "LoopSearchSpaceDimension",
2185  "The size of the search grid used by the matcher.",
2186  8.0, GetParameterManager());
2187 
2189  "LoopSearchSpaceResolution",
2190  "The resolution (size of a grid cell) of the correlation grid.",
2191  0.05, GetParameterManager());
2192 
2194  "LoopSearchSpaceSmearDeviation",
2195  "The point readings are smeared by this value in X and Y to create a "
2196  "smoother response.",
2197  0.03, GetParameterManager());
2198 
2200  // ScanMatcherParameters;
2201 
2203  "DistanceVariancePenalty",
2204  "Variance of penalty for deviating from odometry when scan-matching. "
2205  "The penalty is a multiplier (less than 1.0) is a function of the "
2206  "delta of the scan position being tested and the odometric pose.",
2208 
2210  "AngleVariancePenalty",
2211  "See DistanceVariancePenalty.",
2213 
2215  "FineSearchAngleOffset",
2216  "The range of angles to search during a fine search.",
2218 
2220  "CoarseSearchAngleOffset",
2221  "The range of angles to search during a coarse search.",
2223 
2225  "CoarseAngleResolution",
2226  "Resolution of angles to search during a coarse search.",
2228 
2230  "MinimumAnglePenalty",
2231  "Minimum value of the angle penalty multiplier so scores do not become "
2232  "too small.",
2233  0.9, GetParameterManager());
2234 
2236  "MinimumDistancePenalty",
2237  "Minimum value of the distance penalty multiplier so scores do not "
2238  "become too small.",
2239  0.5, GetParameterManager());
2240 
2242  "UseResponseExpansion",
2243  "Whether to increase the search space if no good matches are initially "
2244  "found.",
2245  false, GetParameterManager());
2246  }
2247  /* Adding in getters and setters here for easy parameter access */
2248 
2249  // General Parameters
2250 
2252  {
2253  return static_cast<bool>(m_pUseScanMatching->GetValue());
2254  }
2255 
2257  {
2258  return static_cast<bool>(m_pUseScanBarycenter->GetValue());
2259  }
2260 
2262  {
2263  return static_cast<double>(m_pMinimumTimeInterval->GetValue());
2264  }
2265 
2267  {
2268  return static_cast<double>(m_pMinimumTravelDistance->GetValue());
2269  }
2270 
2272  {
2273  return math::RadiansToDegrees(static_cast<double>(m_pMinimumTravelHeading->GetValue()));
2274  }
2275 
2277  {
2278  return static_cast<int>(m_pScanBufferSize->GetValue());
2279  }
2280 
2282  {
2283  return static_cast<double>(m_pScanBufferMaximumScanDistance->GetValue());
2284  }
2285 
2287  {
2288  return static_cast<double>(m_pLinkMatchMinimumResponseFine->GetValue());
2289  }
2290 
2292  {
2293  return static_cast<double>(m_pLinkScanMaximumDistance->GetValue());
2294  }
2295 
2297  {
2298  return static_cast<double>(m_pLoopSearchMaximumDistance->GetValue());
2299  }
2300 
2302  {
2303  return static_cast<bool>(m_pDoLoopClosing->GetValue());
2304  }
2305 
2307  {
2308  return static_cast<int>(m_pLoopMatchMinimumChainSize->GetValue());
2309  }
2310 
2312  {
2313  return static_cast<double>(std::sqrt(m_pLoopMatchMaximumVarianceCoarse->GetValue()));
2314  }
2315 
2317  {
2318  return static_cast<double>(m_pLoopMatchMinimumResponseCoarse->GetValue());
2319  }
2320 
2322  {
2323  return static_cast<double>(m_pLoopMatchMinimumResponseFine->GetValue());
2324  }
2325 
2326  // Correlation Parameters - Correlation Parameters
2327 
2329  {
2330  return static_cast<double>(m_pCorrelationSearchSpaceDimension->GetValue());
2331  }
2332 
2334  {
2335  return static_cast<double>(m_pCorrelationSearchSpaceResolution->GetValue());
2336  }
2337 
2339  {
2340  return static_cast<double>(m_pCorrelationSearchSpaceSmearDeviation->GetValue());
2341  }
2342 
2343  // Correlation Parameters - Loop Correlation Parameters
2344 
2346  {
2347  return static_cast<double>(m_pLoopSearchSpaceDimension->GetValue());
2348  }
2349 
2351  {
2352  return static_cast<double>(m_pLoopSearchSpaceResolution->GetValue());
2353  }
2354 
2356  {
2357  return static_cast<double>(m_pLoopSearchSpaceSmearDeviation->GetValue());
2358  }
2359 
2360  // ScanMatcher Parameters
2361 
2363  {
2364  return std::sqrt(static_cast<double>(m_pDistanceVariancePenalty->GetValue()));
2365  }
2366 
2368  {
2369  return std::sqrt(static_cast<double>(m_pAngleVariancePenalty->GetValue()));
2370  }
2371 
2373  {
2374  return static_cast<double>(m_pFineSearchAngleOffset->GetValue());
2375  }
2376 
2378  {
2379  return static_cast<double>(m_pCoarseSearchAngleOffset->GetValue());
2380  }
2381 
2383  {
2384  return static_cast<double>(m_pCoarseAngleResolution->GetValue());
2385  }
2386 
2388  {
2389  return static_cast<double>(m_pMinimumAnglePenalty->GetValue());
2390  }
2391 
2393  {
2394  return static_cast<double>(m_pMinimumDistancePenalty->GetValue());
2395  }
2396 
2398  {
2399  return static_cast<bool>(m_pUseResponseExpansion->GetValue());
2400  }
2401 
2402  /* Setters for parameters */
2403  // General Parameters
2405  {
2407  }
2408 
2410  {
2412  }
2413 
2415  {
2417  }
2418 
2420  {
2422  }
2423 
2425  {
2427  }
2428 
2430  {
2432  }
2433 
2435  {
2437  }
2438 
2440  {
2442  }
2443 
2445  {
2447  }
2448 
2450  {
2452  }
2453 
2455  {
2457  }
2458 
2460  {
2462  }
2463 
2465  {
2467  }
2468 
2470  {
2472  }
2473 
2475  {
2477  }
2478 
2479  // Correlation Parameters - Correlation Parameters
2481  {
2483  }
2484 
2486  {
2488  }
2489 
2491  {
2493  }
2494 
2495 
2496  // Correlation Parameters - Loop Closure Parameters
2498  {
2500  }
2501 
2503  {
2505  }
2506 
2508  {
2510  }
2511 
2512 
2513  // Scan Matcher Parameters
2515  {
2517  }
2518 
2520  {
2522  }
2523 
2525  {
2527  }
2528 
2530  {
2532  }
2533 
2535  {
2537  }
2538 
2540  {
2542  }
2543 
2545  {
2547  }
2548 
2550  {
2552  }
2553 
2554 
2555 
2556  void Mapper::Initialize(kt_double rangeThreshold)
2557  {
2558  if (m_Initialized == false)
2559  {
2560  // create sequential scan and loop matcher
2565  rangeThreshold);
2566  assert(m_pSequentialScanMatcher);
2567 
2570 
2571  m_pGraph = new MapperGraph(this, rangeThreshold);
2572 
2573  m_Initialized = true;
2574  }
2575  }
2576 
2577  void Mapper::SaveToFile(const std::string& filename)
2578  {
2579  printf("Save To File %s \n", filename.c_str());
2580  std::ofstream ofs(filename.c_str());
2581  boost::archive::binary_oarchive oa(ofs, boost::archive::no_codecvt);
2582  oa << BOOST_SERIALIZATION_NVP(*this);
2583  }
2584 
2585  void Mapper::LoadFromFile(const std::string& filename)
2586  {
2587  printf("Load From File %s \n", filename.c_str());
2588  std::ifstream ifs(filename.c_str());
2589  boost::archive::binary_iarchive ia(ifs, boost::archive::no_codecvt);
2590  ia >> BOOST_SERIALIZATION_NVP(*this);
2591  }
2592 
2594  {
2596  {
2597  delete m_pSequentialScanMatcher;
2599  }
2600  if (m_pGraph)
2601  {
2602  delete m_pGraph;
2603  m_pGraph = NULL;
2604  }
2606  {
2607  delete m_pMapperSensorManager;
2609  }
2610  m_Initialized = false;
2611  while (!m_LocalizationScanVertices.empty())
2612  {
2614  }
2615  }
2616 
2618  {
2619  return true;
2620  }
2621 
2623  {
2624  if (pScan != NULL)
2625  {
2626  karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();
2627 
2628  // validate scan
2629  if (pLaserRangeFinder == NULL || pScan == NULL || pLaserRangeFinder->Validate(pScan) == false)
2630  {
2631  return false;
2632  }
2633 
2634  if (m_Initialized == false)
2635  {
2636  // initialize mapper with range threshold from device
2637  Initialize(pLaserRangeFinder->GetRangeThreshold());
2638  }
2639 
2640  // get last scan
2642 
2643  // update scans corrected pose based on last correction
2644  if (pLastScan != NULL)
2645  {
2646  Transform lastTransform(pLastScan->GetOdometricPose(), pLastScan->GetCorrectedPose());
2647  pScan->SetCorrectedPose(lastTransform.TransformPose(pScan->GetOdometricPose()));
2648  }
2649 
2650  // test if scan is outside minimum boundary or if heading is larger then minimum heading
2651  if (!HasMovedEnough(pScan, pLastScan))
2652  {
2653  return false;
2654  }
2655 
2656  Matrix3 covariance;
2657  covariance.SetToIdentity();
2658 
2659  // correct scan (if not first scan)
2660  if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
2661  {
2662  Pose2 bestPose;
2665  bestPose,
2666  covariance);
2667  pScan->SetSensorPose(bestPose);
2668  }
2669 
2670  // add scan to buffer and assign id
2672 
2674  {
2675  // add to graph
2676  m_pGraph->AddVertex(pScan);
2677  m_pGraph->AddEdges(pScan, covariance);
2678 
2680 
2681  if (m_pDoLoopClosing->GetValue())
2682  {
2683  std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
2684  const_forEach(std::vector<Name>, &deviceNames)
2685  {
2686  m_pGraph->TryCloseLoop(pScan, *iter);
2687  }
2688  }
2689  }
2690 
2692 
2693  return true;
2694  }
2695 
2696  return false;
2697  }
2698 
2700  {
2701  if (pScan != NULL)
2702  {
2703  karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();
2704 
2705  // validate scan
2706  if (pLaserRangeFinder == NULL || pScan == NULL ||
2707  pLaserRangeFinder->Validate(pScan) == false)
2708  {
2709  return false;
2710  }
2711 
2712  if (m_Initialized == false)
2713  {
2714  // initialize mapper with range threshold from device
2715  Initialize(pLaserRangeFinder->GetRangeThreshold());
2716  }
2717 
2719  pScan->GetSensorName(), pScan->GetOdometricPose());
2720  LocalizedRangeScan* pLastScan = NULL;
2721  if (closetVertex)
2722  {
2723  pLastScan = m_pMapperSensorManager->GetScan(pScan->GetSensorName(),
2724  closetVertex->GetObject()->GetStateId());
2727  m_pMapperSensorManager->SetLastScan(pLastScan);
2728  }
2729 
2730  Matrix3 covariance;
2731  covariance.SetToIdentity();
2732 
2733  // correct scan (if not first scan)
2734  if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
2735  {
2736  Pose2 bestPose;
2739  bestPose,
2740  covariance);
2741  pScan->SetSensorPose(bestPose);
2742  }
2743 
2744  pScan->SetOdometricPose(pScan->GetCorrectedPose());
2745 
2746  // add scan to buffer and assign id
2748 
2750  {
2751  // add to graph
2752  m_pGraph->AddVertex(pScan);
2753  m_pGraph->AddEdges(pScan, covariance);
2754 
2756 
2757  if (m_pDoLoopClosing->GetValue())
2758  {
2759  std::vector<Name> deviceNames =
2761  const_forEach(std::vector<Name>, &deviceNames)
2762  {
2763  m_pGraph->TryCloseLoop(pScan, *iter);
2764  }
2765  }
2766  }
2767 
2769 
2770  return true;
2771  }
2772 
2773  return false;
2774  }
2775 
2777  {
2778  if (pScan == NULL)
2779  {
2780  return false;
2781  }
2782 
2783  karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();
2784 
2785  // validate scan
2786  if (pLaserRangeFinder == NULL || pScan == NULL ||
2787  pLaserRangeFinder->Validate(pScan) == false)
2788  {
2789  return false;
2790  }
2791 
2792  if (m_Initialized == false)
2793  {
2794  // initialize mapper with range threshold from device
2795  Initialize(pLaserRangeFinder->GetRangeThreshold());
2796  }
2797 
2798  // get last scan
2800  pScan->GetSensorName());
2801 
2802  // update scans corrected pose based on last correction
2803  if (pLastScan != NULL)
2804  {
2805  Transform lastTransform(pLastScan->GetOdometricPose(),
2806  pLastScan->GetCorrectedPose());
2807  pScan->SetCorrectedPose(lastTransform.TransformPose(
2808  pScan->GetOdometricPose()));
2809  }
2810 
2811  // test if scan is outside minimum boundary
2812  // or if heading is larger then minimum heading
2813  if (!HasMovedEnough(pScan, pLastScan))
2814  {
2815  return false;
2816  }
2817 
2818  Matrix3 covariance;
2819  covariance.SetToIdentity();
2820 
2821  // correct scan (if not first scan)
2822  if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
2823  {
2824  Pose2 bestPose;
2827  bestPose,
2828  covariance);
2829  pScan->SetSensorPose(bestPose);
2830  }
2831 
2832  // add scan to buffer and assign id
2834 
2835  Vertex<LocalizedRangeScan>* scan_vertex = NULL;
2837  {
2838  // add to graph
2839  scan_vertex = m_pGraph->AddVertex(pScan);
2840  m_pGraph->AddEdges(pScan, covariance);
2841 
2843 
2844  if (m_pDoLoopClosing->GetValue())
2845  {
2846  std::vector<Name> deviceNames = m_pMapperSensorManager->GetSensorNames();
2847  const_forEach(std::vector<Name>, &deviceNames)
2848  {
2849  m_pGraph->TryCloseLoop(pScan, *iter);
2850  }
2851  }
2852  }
2853 
2855 
2856  // generate the info to store and later decay, outside of dataset
2858  {
2860  RemoveNodeFromGraph(oldLSV.vertex);
2861 
2862  // delete node and scans
2863  // free hat!
2864  // No need to delete from m_scans as those pointers will be freed memory
2865  oldLSV.vertex->RemoveObject();
2867  if (oldLSV.scan)
2868  {
2869  delete oldLSV.scan;
2870  oldLSV.scan = NULL;
2871  }
2872 
2874  }
2875 
2877  lsv.scan = pScan;
2878  lsv.vertex = scan_vertex;
2879  m_LocalizationScanVertices.push(lsv);
2880 
2881  return true;
2882  }
2883 
2885  {
2886  // 1) delete edges in adjacent vertices, graph, and optimizer
2887  std::vector<Vertex<LocalizedRangeScan>*> adjVerts =
2888  vertex_to_remove->GetAdjacentVertices();
2889  for (int i = 0; i != adjVerts.size(); i++)
2890  {
2891  std::vector<Edge<LocalizedRangeScan>*> adjEdges = adjVerts[i]->GetEdges();
2892  bool found = false;
2893  for (int j=0; j!=adjEdges.size(); j++)
2894  {
2895  if (adjEdges[j]->GetTarget() == vertex_to_remove ||
2896  adjEdges[j]->GetSource() == vertex_to_remove)
2897  {
2898  adjVerts[i]->RemoveEdge(j);
2900  adjEdges[j]->GetSource()->GetObject()->GetUniqueId(),
2901  adjEdges[j]->GetTarget()->GetObject()->GetUniqueId());
2902  std::vector<Edge<LocalizedRangeScan>*> edges = m_pGraph->GetEdges();
2903  std::vector<Edge<LocalizedRangeScan>*>::iterator edgeGraphIt =
2904  std::find(edges.begin(), edges.end(), adjEdges[j]);
2905 
2906  if (edgeGraphIt == edges.end())
2907  {
2908  std::cout << "Edge not found in graph to remove!" << std::endl;
2909  continue;
2910  }
2911 
2912  int posEdge = edgeGraphIt - edges.begin();
2913  m_pGraph->RemoveEdge(posEdge); // remove from graph
2914  delete *edgeGraphIt; // free hat!
2915  *edgeGraphIt = NULL;
2916  found = true;
2917  }
2918  }
2919  if (!found)
2920  {
2921  std::cout << "Failed to find any edge in adj. vertex" <<
2922  " with a matching vertex to current!" << std::endl;
2923  }
2924  }
2925 
2926  // 2) delete vertex from optimizer
2927  m_pScanOptimizer->RemoveNode(vertex_to_remove->GetObject()->GetUniqueId());
2928 
2929  // 3) delete from vertex map
2930  std::map<Name, std::map<int, Vertex<LocalizedRangeScan>*> >
2931  vertexMap = m_pGraph->GetVertices();
2932  std::map<int, Vertex<LocalizedRangeScan>*> graphVertices =
2933  vertexMap[vertex_to_remove->GetObject()->GetSensorName()];
2934  std::map<int, Vertex<LocalizedRangeScan>*>::iterator
2935  vertexGraphIt = graphVertices.find(vertex_to_remove->GetObject()->GetStateId());
2936  if (vertexGraphIt != graphVertices.end())
2937  {
2938  m_pGraph->RemoveVertex(vertex_to_remove->GetObject()->GetSensorName(),
2939  vertexGraphIt->second->GetObject()->GetStateId());
2940  }
2941  else
2942  {
2943  std::cout << "Vertex not found in graph to remove!" << std::endl;
2944  return false;
2945  }
2946 
2947  return true;
2948  }
2949 
2951  const int& nodeId)
2952  {
2953  if (pScan != NULL)
2954  {
2955  karto::LaserRangeFinder* pLaserRangeFinder = pScan->GetLaserRangeFinder();
2956 
2957  // validate scan
2958  if (pLaserRangeFinder == NULL || pScan == NULL ||
2959  pLaserRangeFinder->Validate(pScan) == false)
2960  {
2961  return false;
2962  }
2963 
2964  if (m_Initialized == false)
2965  {
2966  // initialize mapper with range threshold from device
2967  Initialize(pLaserRangeFinder->GetRangeThreshold());
2968  }
2969 
2970  // If we're matching against a node from an older mapping session
2971  // lets get the first scan as the last scan and populate running scans
2972  // with the first few from that run as well.
2973  LocalizedRangeScan* pLastScan =
2974  m_pMapperSensorManager->GetScan(pScan->GetSensorName(), nodeId);
2977  m_pMapperSensorManager->SetLastScan(pLastScan);
2978 
2979  Matrix3 covariance;
2980  covariance.SetToIdentity();
2981 
2982  // correct scan (if not first scan)
2983  if (m_pUseScanMatching->GetValue() && pLastScan != NULL)
2984  {
2985  Pose2 bestPose;
2988  bestPose,
2989  covariance);
2990  pScan->SetSensorPose(bestPose);
2991  }
2992 
2993  pScan->SetOdometricPose(pScan->GetCorrectedPose());
2994 
2995  // add scan to buffer and assign id
2997 
2999  {
3000  // add to graph
3001  m_pGraph->AddVertex(pScan);
3002  m_pGraph->AddEdges(pScan, covariance);
3003 
3005 
3006  if (m_pDoLoopClosing->GetValue())
3007  {
3008  std::vector<Name> deviceNames =
3010  const_forEach(std::vector<Name>, &deviceNames)
3011  {
3012  m_pGraph->TryCloseLoop(pScan, *iter);
3013  }
3014  }
3015  }
3016 
3018 
3019  return true;
3020  }
3021 
3022  return false;
3023  }
3024 
3026  {
3027  // Special case of processing against node where node is the starting point
3028  return ProcessAgainstNode(pScan, 0);
3029  }
3030 
3038  {
3039  // test if first scan
3040  if (pLastScan == NULL)
3041  {
3042  return true;
3043  }
3044 
3045  // test if enough time has passed
3046  kt_double timeInterval = pScan->GetTime() - pLastScan->GetTime();
3047  if (timeInterval >= m_pMinimumTimeInterval->GetValue())
3048  {
3049  return true;
3050  }
3051 
3052  Pose2 lastScannerPose = pLastScan->GetSensorAt(pLastScan->GetOdometricPose());
3053  Pose2 scannerPose = pScan->GetSensorAt(pScan->GetOdometricPose());
3054 
3055  // test if we have turned enough
3056  kt_double deltaHeading = math::NormalizeAngle(scannerPose.GetHeading() - lastScannerPose.GetHeading());
3057  if (fabs(deltaHeading) >= m_pMinimumTravelHeading->GetValue())
3058  {
3059  return true;
3060  }
3061 
3062  // test if we have moved enough
3063  kt_double squaredTravelDistance = lastScannerPose.GetPosition().SquaredDistance(scannerPose.GetPosition());
3064  if (squaredTravelDistance >= math::Square(m_pMinimumTravelDistance->GetValue()) - KT_TOLERANCE)
3065  {
3066  return true;
3067  }
3068 
3069  return false;
3070  }
3071 
3077  {
3078  LocalizedRangeScanVector allScans;
3079 
3081  {
3082  allScans = m_pMapperSensorManager->GetAllScans();
3083  }
3084 
3085  return allScans;
3086  }
3087 
3093  {
3094  m_Listeners.push_back(pListener);
3095  }
3096 
3102  {
3103  std::vector<MapperListener*>::iterator iter = std::find(m_Listeners.begin(), m_Listeners.end(), pListener);
3104  if (iter != m_Listeners.end())
3105  {
3106  m_Listeners.erase(iter);
3107  }
3108  }
3109 
3110  void Mapper::FireInfo(const std::string& rInfo) const
3111  {
3112  const_forEach(std::vector<MapperListener*>, &m_Listeners)
3113  {
3114  (*iter)->Info(rInfo);
3115  }
3116  }
3117 
3118  void Mapper::FireDebug(const std::string& rInfo) const
3119  {
3120  const_forEach(std::vector<MapperListener*>, &m_Listeners)
3121  {
3122  MapperDebugListener* pListener = dynamic_cast<MapperDebugListener*>(*iter);
3123 
3124  if (pListener != NULL)
3125  {
3126  pListener->Debug(rInfo);
3127  }
3128  }
3129  }
3130 
3131  void Mapper::FireLoopClosureCheck(const std::string& rInfo) const
3132  {
3133  const_forEach(std::vector<MapperListener*>, &m_Listeners)
3134  {
3135  MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
3136 
3137  if (pListener != NULL)
3138  {
3139  pListener->LoopClosureCheck(rInfo);
3140  }
3141  }
3142  }
3143 
3144  void Mapper::FireBeginLoopClosure(const std::string& rInfo) const
3145  {
3146  const_forEach(std::vector<MapperListener*>, &m_Listeners)
3147  {
3148  MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
3149 
3150  if (pListener != NULL)
3151  {
3152  pListener->BeginLoopClosure(rInfo);
3153  }
3154  }
3155  }
3156 
3157  void Mapper::FireEndLoopClosure(const std::string& rInfo) const
3158  {
3159  const_forEach(std::vector<MapperListener*>, &m_Listeners)
3160  {
3161  MapperLoopClosureListener* pListener = dynamic_cast<MapperLoopClosureListener*>(*iter);
3162 
3163  if (pListener != NULL)
3164  {
3165  pListener->EndLoopClosure(rInfo);
3166  }
3167  }
3168  }
3169 
3170  void Mapper::SetScanSolver(ScanSolver* pScanOptimizer)
3171  {
3172  m_pScanOptimizer = pScanOptimizer;
3173  }
3174 
3176  {
3177  return m_pScanOptimizer;
3178  }
3179 
3181  {
3182  return m_pGraph;
3183  }
3184 
3186  {
3187  return m_pSequentialScanMatcher;
3188  }
3189 
3191  {
3192  return m_pGraph->GetLoopScanMatcher();
3193  }
3194 } // namespace karto
std::vector< LocalizedRangeScanVector > FindNearChains(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:1656
virtual kt_bool Process(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:2622
std::map< Name, ScanManager * > ScanManagerMap
Definition: Mapper.h:1519
T GetHeight() const
Definition: Karto.h:1923
void InitializeParameters()
Definition: Mapper.cpp:2040
void RemoveScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:176
LaserRangeFinder * GetLaserRangeFinder() const
Definition: Karto.h:5395
#define NULL
virtual std::vector< Vertex< T > * > TraverseForVertices(Vertex< T > *pStartVertex, Visitor< T > *pVisitor)
Definition: Mapper.cpp:1229
ScanMatcher * m_pSequentialScanMatcher
Definition: Mapper.h:2097
void RemoveListener(MapperListener *pListener)
Definition: Mapper.cpp:3101
virtual kt_bool Visit(Vertex< LocalizedRangeScan > *pVertex)
Definition: Mapper.cpp:1334
double getParamMinimumAnglePenalty()
Definition: Mapper.cpp:2387
double getParamLinkScanMaximumDistance()
Definition: Mapper.cpp:2291
#define DISTANCE_PENALTY_GAIN
Definition: Mapper.cpp:47
bool getParamDoLoopClosing()
Definition: Mapper.cpp:2301
int32_t kt_int32s
Definition: Types.h:51
virtual kt_bool Validate()
Definition: Karto.h:4150
void setParamCorrelationSearchSpaceResolution(double d)
Definition: Mapper.cpp:2485
void FireEndLoopClosure(const std::string &rInfo) const
Definition: Mapper.cpp:3157
void setParamLoopMatchMinimumChainSize(int i)
Definition: Mapper.cpp:2459
Matrix3 Inverse() const
Definition: Karto.h:2543
const kt_int32s INVALID_SCAN
Definition: Math.h:47
Parameter< kt_double > * m_pLinkScanMaximumDistance
Definition: Mapper.h:2184
kt_double Round(kt_double value)
Definition: Math.h:87
void LinkNearChains(LocalizedRangeScan *pScan, Pose2Vector &rMeans, std::vector< Matrix3 > &rCovariances)
Definition: Mapper.cpp:1616
virtual void AddNode(Vertex< LocalizedRangeScan > *)
Definition: Mapper.h:978
void SetUniqueId(kt_int32u uniqueId)
Definition: Karto.h:5170
Parameter< kt_double > * m_pCoarseAngleResolution
Definition: Mapper.h:2283
Pose2 GetSensorPose() const
Definition: Karto.h:5621
kt_int32s GetStateId() const
Definition: Karto.h:5143
virtual ScanMatcher * GetLoopScanMatcher() const
Definition: Mapper.cpp:3190
NearScanVisitor(LocalizedRangeScan *pScan, kt_double maxDistance, kt_bool useScanBarycenter)
Definition: Mapper.cpp:1281
const Pose2 & GetOdometricPose() const
Definition: Karto.h:5538
Parameter< kt_double > * m_pLoopMatchMaximumVarianceCoarse
Definition: Mapper.h:2212
std::vector< Vector2< kt_double > > PointVectorDouble
Definition: Karto.h:1281
Parameter< kt_double > * m_pMinimumTravelHeading
Definition: Mapper.h:2154
virtual void EndLoopClosure(const std::string &)
Definition: Mapper.h:96
void SetValue(const T &rValue)
Definition: Karto.h:3280
void setParamUseScanMatching(bool b)
Definition: Mapper.cpp:2404
void AddEdge(Edge< T > *pEdge)
Definition: Mapper.h:620
kt_bool ProcessAgainstNode(LocalizedRangeScan *pScan, const int &nodeId)
Definition: Mapper.cpp:2950
int getParamLoopMatchMinimumChainSize()
Definition: Mapper.cpp:2306
void ComputeAngularCovariance(const Pose2 &rBestPose, kt_double bestResponse, const Pose2 &rSearchCenter, kt_double searchAngleOffset, kt_double searchAngleResolution, Matrix3 &rCovariance)
Definition: Mapper.cpp:933
Parameter< kt_double > * m_pMinimumAnglePenalty
Definition: Mapper.h:2287
std::vector< MapperListener * > m_Listeners
Definition: Mapper.h:2106
virtual void Clear()
Definition: Mapper.h:1006
double getParamCoarseSearchAngleOffset()
Definition: Mapper.cpp:2377
void FireBeginLoopClosure(const std::string &rInfo) const
Definition: Mapper.cpp:3144
kt_bool m_UseScanBarycenter
Definition: Mapper.cpp:1308
kt_int32s * GetArrayPointer()
Definition: Karto.h:6815
virtual void RemoveNode(kt_int32s)
Definition: Mapper.h:985
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5560
kt_int32u GetNumberOfRangeReadings() const
Definition: Karto.h:5404
const Vector2< kt_double > & GetPosition() const
Definition: Karto.h:2133
uint8_t kt_int8u
Definition: Types.h:46
const T & Maximum(const T &value1, const T &value2)
Definition: Math.h:111
virtual void RemoveConstraint(kt_int32s, kt_int32s)
Definition: Mapper.h:999
kt_double DegreesToRadians(kt_double degrees)
Definition: Math.h:56
double getParamCoarseAngleResolution()
Definition: Mapper.cpp:2382
friend class MapperGraph
Definition: Mapper.h:1884
std::map< Name, std::map< int, Vertex< LocalizedRangeScan > *> > VertexMap
Definition: Mapper.h:567
std::vector< Name > GetSensorNames()
Definition: Mapper.h:1563
Vertex< LocalizedRangeScan > * vertex
Definition: Mapper.h:1877
double getParamMinimumDistancePenalty()
Definition: Mapper.cpp:2392
void ClearRunningScans()
Definition: Mapper.cpp:193
virtual void LoopClosureCheck(const std::string &)
Definition: Mapper.h:86
void SetX(const T &x)
Definition: Karto.h:1035
Parameter< kt_double > * m_pLoopSearchSpaceSmearDeviation
Definition: Mapper.h:2267
kt_bool ProcessAgainstNodesNearBy(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:2699
kt_int32u GetSize() const
Definition: Karto.h:6761
void setParamDoLoopClosing(bool b)
Definition: Mapper.cpp:2454
const std::vector< Edge< T > * > & GetEdges() const
Definition: Mapper.h:665
virtual kt_bool Visit(Vertex< LocalizedRangeScan > *pVertex)
Definition: Mapper.cpp:1288
const PointVectorDouble & GetPointReadings(kt_bool wantFiltered=false) const
Definition: Karto.h:5687
Parameter< kt_int32u > * m_pLoopMatchMinimumChainSize
Definition: Mapper.h:2205
void LoadFromFile(const std::string &filename)
Definition: Mapper.cpp:2585
void setParamCorrelationSearchSpaceSmearDeviation(double d)
Definition: Mapper.cpp:2490
Parameter< kt_double > * m_pCorrelationSearchSpaceResolution
Definition: Mapper.h:2239
void Initialize(kt_double rangeThreshold)
Definition: Mapper.cpp:2556
double getParamMinimumTimeInterval()
Definition: Mapper.cpp:2261
Parameter< kt_double > * m_pMinimumDistancePenalty
Definition: Mapper.h:2288
LocalizedRangeScanVector FindNearByScans(Name name, const Pose2 refPose, kt_double maxDistance)
Definition: Mapper.cpp:1785
void setParamDistanceVariancePenalty(double d)
Definition: Mapper.cpp:2514
void AddVertex(const Name &rName, Vertex< T > *pVertex)
Definition: Mapper.h:591
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:827
virtual void AddConstraint(Edge< LocalizedRangeScan > *)
Definition: Mapper.h:992
Parameter< kt_double > * m_pDistanceVariancePenalty
Definition: Mapper.h:2275
void SetStateId(kt_int32s stateId)
Definition: Karto.h:5152
Parameter< kt_bool > * m_pUseScanMatching
Definition: Mapper.h:2116
Vertex< T > * GetTarget() const
Definition: Mapper.h:448
Pose2 ComputeWeightedMean(const Pose2Vector &rMeans, const std::vector< Matrix3 > &rCovariances) const
Definition: Mapper.cpp:1877
LocalizedRangeScanVector & GetRunningScans(const Name &rSensorName)
Definition: Mapper.cpp:345
#define forEach(listtype, list)
Definition: Macros.h:66
void setParamScanBufferSize(int i)
Definition: Mapper.cpp:2429
void AddScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:294
void setParamCorrelationSearchSpaceDimension(double d)
Definition: Mapper.cpp:2480
void SetSensorPose(const Pose2 &rScanPose)
Definition: Karto.h:5635
LocalizedRangeScan * GetLastScan()
Definition: Mapper.cpp:104
Pose2 GetSensorAt(const Pose2 &rPose) const
Definition: Karto.h:5656
void setParamMinimumTimeInterval(double d)
Definition: Mapper.cpp:2414
kt_double GetResponse(kt_int32u angleIndex, kt_int32s gridPositionIndex) const
Definition: Mapper.cpp:1137
T * GetObject() const
Definition: Mapper.h:319
void SetLastScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:285
kt_double m_MaxDistanceSquared
Definition: Mapper.cpp:1346
LocalizedRangeScanVector m_RunningScans
Definition: Mapper.cpp:222
kt_int32s GetUniqueId() const
Definition: Karto.h:5161
#define ANGLE_PENALTY_GAIN
Definition: Mapper.cpp:48
void LinkChainToScan(const LocalizedRangeScanVector &rChain, LocalizedRangeScan *pScan, const Pose2 &rMean, const Matrix3 &rCovariance)
Definition: Mapper.cpp:1639
void SetToIdentity()
Definition: Karto.h:2465
double getParamLoopMatchMinimumResponseCoarse()
Definition: Mapper.cpp:2316
void LinkScans(LocalizedRangeScan *pFromScan, LocalizedRangeScan *pToScan, const Pose2 &rMean, const Matrix3 &rCovariance)
Definition: Mapper.cpp:1594
MapperSensorManager * m_pMapperSensorManager
Definition: Mapper.h:2099
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
GraphTraversal< LocalizedRangeScan > * m_pTraversal
Definition: Mapper.h:913
double getParamLoopMatchMinimumResponseFine()
Definition: Mapper.cpp:2321
Parameter< kt_bool > * m_pDoLoopClosing
Definition: Mapper.h:2190
virtual kt_bool Visit(Vertex< T > *pVertex)=0
BreadthFirstTraversal(Graph< T > *pGraph)
Definition: Mapper.cpp:1191
virtual ScanMatcher * GetSequentialScanMatcher() const
Definition: Mapper.cpp:3185
void setParamAngleVariancePenalty(double d)
Definition: Mapper.cpp:2519
Parameter< kt_double > * m_pFineSearchAngleOffset
Definition: Mapper.h:2279
std::vector< Vertex< LocalizedRangeScan > * > FindNearByVertices(Name name, const Pose2 refPose, kt_double maxDistance)
Definition: Mapper.cpp:1797
kt_int32u m_RunningBufferMaximumSize
Definition: Mapper.cpp:226
LocalizedRangeScan * GetClosestScanToPose(const LocalizedRangeScanVector &rScans, const Pose2 &rPose) const
Definition: Mapper.cpp:1542
kt_double SquaredLength() const
Definition: Karto.h:1082
kt_double RadiansToDegrees(kt_double radians)
Definition: Math.h:66
Parameter< kt_double > * m_pScanBufferMaximumScanDistance
Definition: Mapper.h:2171
ScanSolver * m_pScanOptimizer
Definition: Mapper.h:2102
LocalizedRangeScan * scan
Definition: Mapper.h:1875
void setParamLoopMatchMaximumVarianceCoarse(double d)
Definition: Mapper.cpp:2464
virtual void Debug(const std::string &)
Definition: Mapper.h:69
kt_int32u & GetRunningScanBufferSize()
Definition: Mapper.cpp:140
void AddListener(MapperListener *pListener)
Definition: Mapper.cpp:3092
Parameter< kt_double > * m_pLoopSearchSpaceDimension
Definition: Mapper.h:2255
void setParamLinkScanMaximumDistance(double d)
Definition: Mapper.cpp:2444
kt_double SquaredDistance(const Vector2 &rOther) const
Definition: Karto.h:1100
void SetLastScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:113
friend class boost::serialization::access
Definition: Mapper.cpp:208
void setParamMinimumAnglePenalty(double d)
Definition: Mapper.cpp:2539
kt_double GetRangeThreshold() const
Definition: Karto.h:3983
virtual MapperGraph * GetGraph() const
Definition: Mapper.cpp:3180
virtual ParameterManager * GetParameterManager()
Definition: Karto.h:677
const Name & GetSensorName() const
Definition: Karto.h:5197
virtual const IdPoseVector & GetCorrections() const =0
void ClearRunningScans(const Name &rSensorName)
Definition: Mapper.cpp:350
double getParamCorrelationSearchSpaceSmearDeviation()
Definition: Mapper.cpp:2338
ScanManager(kt_int32u runningBufferMaximumSize, kt_double runningBufferMaximumDistance)
Definition: Mapper.cpp:63
Parameter< kt_double > * m_pMinimumTravelDistance
Definition: Mapper.h:2144
kt_bool ProcessLocalization(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:2776
void AddRunningScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:305
virtual ~Mapper()
Definition: Mapper.cpp:2033
void AddRunningScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:149
virtual std::vector< T * > TraverseForScans(Vertex< T > *pStartVertex, Visitor< T > *pVisitor)
Definition: Mapper.cpp:1210
RangeReadingsVector GetRangeReadingsVector() const
Definition: Karto.h:5343
Mapper * m_pMapper
Definition: Mapper.h:903
double getParamMinimumTravelHeading()
Definition: Mapper.cpp:2271
void setParamLoopSearchSpaceDimension(double d)
Definition: Mapper.cpp:2497
void setParamLoopSearchSpaceResolution(double d)
Definition: Mapper.cpp:2502
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
static Grid * CreateGrid(kt_int32s width, kt_int32s height, kt_double resolution)
Definition: Karto.h:4653
void setParamScanBufferMaximumScanDistance(double d)
Definition: Mapper.cpp:2434
void setParamMinimumDistancePenalty(double d)
Definition: Mapper.cpp:2544
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.cpp:1311
kt_double GetX() const
Definition: Karto.h:2097
LocalizedRangeScanVector & GetRunningScans()
Definition: Mapper.cpp:131
double getParamMinimumTravelDistance()
Definition: Mapper.cpp:2266
kt_bool m_UseScanBarycenter
Definition: Mapper.cpp:1347
std::vector< Pose2 > Pose2Vector
Definition: Karto.h:2269
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.cpp:1268
double getParamLoopSearchSpaceSmearDeviation()
Definition: Mapper.cpp:2355
ScanMatcher * GetLoopScanMatcher() const
Definition: Mapper.h:806
ScanSolver * getScanSolver()
Definition: Mapper.cpp:3175
double getParamFineSearchAngleOffset()
Definition: Mapper.cpp:2372
void setParamLoopSearchMaximumDistance(double d)
Definition: Mapper.cpp:2449
const T & GetValue() const
Definition: Karto.h:3271
T GetWidth() const
Definition: Karto.h:1905
kt_bool ProcessAtDock(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:3025
void setParamLoopMatchMinimumResponseCoarse(double d)
Definition: Mapper.cpp:2469
kt_bool HasMovedEnough(LocalizedRangeScan *pScan, LocalizedRangeScan *pLastScan) const
Definition: Mapper.cpp:3037
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.cpp:210
void RemoveVertex(const Name &rName, const int &idx)
Definition: Mapper.h:601
PointVectorDouble FindValidPoints(LocalizedRangeScan *pScan, const Vector2< kt_double > &rViewPoint) const
Definition: Mapper.cpp:1074
bool getParamUseScanBarycenter()
Definition: Mapper.cpp:2256
void FireDebug(const std::string &rInfo) const
Definition: Mapper.cpp:3118
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:172
void AddScan(LocalizedRangeScan *pScan, kt_int32s uniqueId)
Definition: Mapper.cpp:86
void setParamMinimumTravelDistance(double d)
Definition: Mapper.cpp:2419
const kt_double KT_TOLERANCE
Definition: Math.h:41
kt_double NormalizeAngle(kt_double angle)
Definition: Math.h:182
const T & GetX() const
Definition: Karto.h:1026
LocalizedRangeScan * GetScan(const Name &rSensorName, kt_int32s scanIndex)
Definition: Mapper.cpp:249
bool getParamUseScanMatching()
Definition: Mapper.cpp:2251
#define const_forEach(listtype, list)
Definition: Macros.h:72
LocalizationScanVertices m_LocalizationScanVertices
Definition: Mapper.h:2103
double getParamScanBufferMaximumScanDistance()
Definition: Mapper.cpp:2281
void SetScanSolver(ScanSolver *pSolver)
Definition: Mapper.cpp:3170
virtual void BeginLoopClosure(const std::string &)
Definition: Mapper.h:91
LocalizedRangeScanVector FindPossibleLoopClosure(LocalizedRangeScan *pScan, const Name &rSensorName, kt_int32u &rStartNum)
Definition: Mapper.cpp:1921
int getParamScanBufferSize()
Definition: Mapper.cpp:2276
bool getParamUseResponseExpansion()
Definition: Mapper.cpp:2397
double getParamCorrelationSearchSpaceDimension()
Definition: Mapper.cpp:2328
void SaveToFile(const std::string &filename)
Definition: Mapper.cpp:2577
std::vector< Vertex< T > * > GetAdjacentVertices() const
Definition: Mapper.h:336
GridIndexLookup< kt_int8u > * m_pGridLookup
Definition: Mapper.h:1470
LocalizedRangeScanMap & GetScans(const Name &rSensorName)
Definition: Mapper.cpp:335
Pose2 GetReferencePose(kt_bool useBarycenter) const
Definition: Karto.h:5603
void SetOdometricPose(const Pose2 &rPose)
Definition: Karto.h:5547
virtual ~ScanMatcher()
Definition: Mapper.cpp:403
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:1081
kt_double GetTime() const
Definition: Karto.h:5179
void AddScans(const LocalizedRangeScanVector &rScans, Vector2< kt_double > viewPoint)
Definition: Mapper.cpp:991
void serialize(Archive &ar, const unsigned int version)
Definition: Mapper.cpp:1350
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:1034
void setParamLinkMatchMinimumResponseFine(double d)
Definition: Mapper.cpp:2439
CorrelationGrid * m_pCorrelationGrid
Definition: Mapper.h:1468
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:654
Parameter< kt_double > * m_pLoopSearchSpaceResolution
Definition: Mapper.h:2261
#define MAX_VARIANCE
Definition: Mapper.cpp:46
kt_bool m_Initialized
Definition: Mapper.h:2095
void RemoveEdge(const int &idx)
Definition: Mapper.h:629
Parameter< kt_double > * m_pCorrelationSearchSpaceDimension
Definition: Mapper.h:2233
kt_int32u GetRunningScanBufferSize(const Name &rSensorName)
Definition: Mapper.cpp:356
void setParamCoarseSearchAngleOffset(double d)
Definition: Mapper.cpp:2529
double getParamDistanceVariancePenalty()
Definition: Mapper.cpp:2362
double getParamCorrelationSearchSpaceResolution()
Definition: Mapper.cpp:2333
kt_double MatchScan(LocalizedRangeScan *pScan, const T &rBaseScans, Pose2 &rMean, Matrix3 &rCovariance, kt_bool doPenalize=true, kt_bool doRefineMatch=true)
Definition: Mapper.cpp:479
void RegisterSensor(const Name &rSensorName)
Definition: Mapper.cpp:234
std::vector< std::pair< kt_int32s, Pose2 > > IdPoseVector
Definition: Mapper.h:947
void SetCorrectedPose(const Pose2 &rPose)
Definition: Karto.h:5569
LocalizedRangeScanVector GetAllScans()
Definition: Mapper.cpp:365
void setParamFineSearchAngleOffset(double d)
Definition: Mapper.cpp:2524
void RemoveScan(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:314
Parameter< kt_double > * m_pLinkMatchMinimumResponseFine
Definition: Mapper.h:2177
Vertex< LocalizedRangeScan > * GetVertex(LocalizedRangeScan *pScan)
Definition: Mapper.h:817
void FireInfo(const std::string &rInfo) const
Definition: Mapper.cpp:3110
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
T Square(T value)
Definition: Math.h:77
virtual ~ScanManager()
Definition: Mapper.cpp:76
BOOST_CLASS_EXPORT(karto::MapperGraph)
double getParamAngleVariancePenalty()
Definition: Mapper.cpp:2367
static ScanMatcher * Create(Mapper *pMapper, kt_double searchSize, kt_double resolution, kt_double smearDeviation, kt_double rangeThreshold)
Definition: Mapper.cpp:420
ScanMatcher * m_pLoopScanMatcher
Definition: Mapper.h:908
void SetLabel(EdgeLabel *pLabel)
Definition: Mapper.h:466
Grid< kt_double > * m_pSearchSpaceProbs
Definition: Mapper.h:1469
kt_bool DoubleEqual(kt_double a, kt_double b)
Definition: Math.h:135
Edge< LocalizedRangeScan > * AddEdge(LocalizedRangeScan *pSourceScan, LocalizedRangeScan *pTargetScan, kt_bool &rIsNewEdge)
Definition: Mapper.cpp:1563
void SetHeading(kt_double heading)
Definition: Karto.h:2160
LocalizedRangeScanVector FindNearLinkedScans(LocalizedRangeScan *pScan, kt_double maxDistance)
Definition: Mapper.cpp:1767
double kt_double
Definition: Types.h:67
double getParamLoopSearchSpaceResolution()
Definition: Mapper.cpp:2350
Parameter< kt_double > * m_pCorrelationSearchSpaceSmearDeviation
Definition: Mapper.h:2245
Definition: Karto.h:86
void setParamMinimumTravelHeading(double d)
Definition: Mapper.cpp:2424
const T & GetY() const
Definition: Karto.h:1044
void AddEdges(LocalizedRangeScan *pScan, const Matrix3 &rCovariance)
Definition: Mapper.cpp:1405
double getParamLoopSearchMaximumDistance()
Definition: Mapper.cpp:2296
void setParamUseResponseExpansion(bool b)
Definition: Mapper.cpp:2549
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Vertex< LocalizedRangeScan > * AddVertex(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:1389
virtual void Reset()
Definition: Mapper.cpp:2593
kt_double GetY() const
Definition: Karto.h:2115
virtual void Compute()=0
MapperGraph * m_pGraph
Definition: Mapper.h:2101
Parameter< kt_double > * m_pLoopMatchMinimumResponseCoarse
Definition: Mapper.h:2218
void FireLoopClosureCheck(const std::string &rInfo) const
Definition: Mapper.cpp:3131
double getParamLoopMatchMaximumVarianceCoarse()
Definition: Mapper.cpp:2311
kt_bool TryCloseLoop(LocalizedRangeScan *pScan, const Name &rSensorName)
Definition: Mapper.cpp:1476
LocalizedRangeScan * m_pLastScan
Definition: Mapper.cpp:223
double getParamLinkMatchMinimumResponseFine()
Definition: Mapper.cpp:2286
virtual ~MapperGraph()
Definition: Mapper.cpp:1375
void operator()(const kt_double &y) const
Definition: Mapper.cpp:584
kt_double m_RunningBufferMaximumDistance
Definition: Mapper.cpp:227
const VertexMap & GetVertices() const
Definition: Mapper.h:674
Parameter< kt_double > * m_pCoarseSearchAngleOffset
Definition: Mapper.h:2280
void setParamLoopMatchMinimumResponseFine(double d)
Definition: Mapper.cpp:2474
std::vector< Vertex< LocalizedRangeScan > * > FindNearLinkedVertices(LocalizedRangeScan *pScan, kt_double maxDistance)
Definition: Mapper.cpp:1776
Parameter< kt_double > * m_pLoopSearchMaximumDistance
Definition: Mapper.h:2197
NearPoseVisitor(Pose2 refPose, kt_double maxDistance, kt_bool useScanBarycenter)
Definition: Mapper.cpp:1327
void setParamUseScanBarycenter(bool b)
Definition: Mapper.cpp:2409
Parameter< kt_double > * m_pAngleVariancePenalty
Definition: Mapper.h:2276
void SetY(const T &y)
Definition: Karto.h:1053
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
uint32_t kt_int32u
Definition: Types.h:52
double getParamLoopSearchSpaceDimension()
Definition: Mapper.cpp:2345
LocalizedRangeScanMap m_Scans
Definition: Mapper.cpp:221
LocalizedRangeScanMap & GetScans()
Definition: Mapper.cpp:122
Parameter< kt_double > * m_pMinimumTimeInterval
Definition: Mapper.h:2134
Parameter< kt_bool > * m_pUseResponseExpansion
Definition: Mapper.h:2291
Parameter< kt_int32u > * m_pScanBufferSize
Definition: Mapper.h:2164
virtual const LocalizedRangeScanVector GetAllProcessedScans() const
Definition: Mapper.cpp:3076
kt_bool IsUpTo(const T &value, const T &maximum)
Definition: Math.h:147
const std::string response
LocalizedRangeScan * GetLastScan(const Name &rSensorName)
Definition: Mapper.cpp:274
void setParamLoopSearchSpaceSmearDeviation(double d)
Definition: Mapper.cpp:2507
kt_int32u m_NextStateId
Definition: Mapper.cpp:224
std::vector< LocalizedRangeScan * > LocalizedRangeScanVector
Definition: Karto.h:5856
Parameter< kt_bool > * m_pUseScanBarycenter
Definition: Mapper.h:2121
std::map< int, LocalizedRangeScan * > LocalizedRangeScanMap
Definition: Karto.h:5857
Parameter< kt_double > * m_pLoopMatchMinimumResponseFine
Definition: Mapper.h:2224
void setParamCoarseAngleResolution(double d)
Definition: Mapper.cpp:2534
Vertex< LocalizedRangeScan > * FindNearByScan(Name name, const Pose2 refPose)
Definition: Mapper.cpp:1836
kt_double GetHeading() const
Definition: Karto.h:2151
kt_bool RemoveNodeFromGraph(Vertex< LocalizedRangeScan > *)
Definition: Mapper.cpp:2884
kt_double m_MaxDistanceSquared
Definition: Mapper.cpp:1307


slam_toolbox
Author(s): Steve Macenski
autogenerated on Sat Oct 3 2020 03:51:01