karto slam

输入：scan、tf（necessary：laser、base、odometry）

输出：map

How to use Karto stand-alone (tutorial1.cpp)

1. 创建一个LaserRangeFinder类对象

 // Create a laser range finder device - use SmartPointer to let karto subsystem manage memory.
  karto::Name name("laser0");
  karto::LaserRangeFinder* pLaserRangeFinder = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, name);
  pLaserRangeFinder->SetOffsetPose(karto::Pose2(1.0, 0.0, 0.0));
  pLaserRangeFinder->SetAngularResolution(karto::math::DegreesToRadians(0.5));
  pLaserRangeFinder->SetRangeThreshold(12.0);

定义了一个自定义的激光测距仪，主要包括名字，教分辨率，视野，最大测距范围，位姿等设置

2. LocalizedRangScan

Karto有自己的激光数据结构。此数据结构带上了位置信息

// 1. localized range scan

  // Create a vector of range readings. Simple example where all the measurements are the same value.
  std::vector<kt_double> readings;
  for (int i=0; i<361; i++)
  {
    readings.push_back(3.0);
  }
  
  // create localized range scan
  pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
  pLocalizedRangeScan->SetOdometricPose(karto::Pose2(0.0, 0.0, 0.0));
  pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(0.0, 0.0, 0.0));

刚刚正确的创建了激光测距仪装置和激光测距仪的数据结构对应的对像，现在就可以call mapper 得到正确的位置了。

3. call slam

  // Get karto default mapper
  karto::Mapper* pMapper = new karto::Mapper();
.... 
// Create the localized range scan as described above
....
 // Add the localized range scan to the mapper
  pMapper->Process(pLocalizedRangeScan);
  std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;

4. get the map

刚刚我们已经加载了激光装置，并且把数据放在了激光的数据结构中，并且call slam mapper

对数据进行了处理Process，然后就可以调用

 karto::OccupancyGrid* pOccupancyGrid = karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution);

得到地图了。

接下来可以可视化地图数据。

5. 创建数据集

// create an empty dataset
karto::Dataset* pDataset = new karto::Dataset();
.... 
// Create the localized range scan as described above
....
// Add the localized range scan to the dataset
pDataset->Add(pLocalizedRangeScan);

// write dataset to file
WriteDataset(pDataset, "tutorial1.kxd");

可以用来记录数据，可以Debug，可以反馈信息，没有看出和建图应用的直接关系。

6. 循环闭环求解器

// Get karto default mapper
karto::Mapper* pMapper = new karto::Mapper();

// set solver
SpaSolver* pSolver = new SpaSolver();
pMapper->SetScanSolver(pSolver);