18 #include <OpenKarto/Mapper.h> 37 pDataset->
Add(pLaserRangeFinder);
46 std::vector<kt_double> readings;
47 for (
int i=0; i<361; i++)
49 readings.push_back(3.0);
58 pMapper->
Process(pLocalizedRangeScan);
62 pDataset->
Add(pLocalizedRangeScan);
72 pMapper->
Process(pLocalizedRangeScan);
76 pDataset->
Add(pLocalizedRangeScan);
86 pMapper->
Process(pLocalizedRangeScan);
90 pDataset->
Add(pLocalizedRangeScan);
100 std::cout <<
"Generating map..." << std::endl;
105 return pOccupancyGrid;
113 if (pOccupancyGrid != NULL)
120 std::cout <<
"width = " << width <<
", height = " << height <<
", scale = " << pOccupancyGrid->
GetCoordinateConverter()->
GetScale() <<
", offset: " << offset.
GetX() <<
", " << offset.
GetY() << std::endl;
143 std::cout << std::endl;
145 std::cout << std::endl;
162 std::cout <<
"Tutorial 1 ----------------" << std::endl << std::endl;
173 delete pOccupancyGrid;
kt_int32s GetHeight() const
const Pose2 & GetOdometricPose() const
virtual kt_bool Process(LocalizedRangeScan *pScan)
void SetRangeThreshold(kt_double rangeThreshold)
karto::OccupancyGrid * CreateOccupancyGrid(karto::Mapper *pMapper, kt_double resolution)
kt_double DegreesToRadians(kt_double degrees)
kt_int32s GetWidth() const
void SetOffsetPose(const Pose2 &rPose)
void SetAngularResolution(kt_double angularResolution)
void Add(Object *pObject)
karto::Dataset * CreateMap(karto::Mapper *pMapper)
void PrintOccupancyGrid(karto::OccupancyGrid *pOccupancyGrid)
static OccupancyGrid * CreateFromScans(const LocalizedRangeScanVector &rScans, kt_double resolution)
virtual const LocalizedRangeScanVector GetAllProcessedScans() const
void SetOdometricPose(const Pose2 &rPose)
static LaserRangeFinder * CreateLaserRangeFinder(LaserRangeFinderType type, const Name &rName)
const Vector2< kt_double > & GetOffset() const
CoordinateConverter * GetCoordinateConverter() const
void SetCorrectedPose(const Pose2 &rPose)
T GetValue(const Vector2< kt_int32s > &rGrid) const
kt_double GetScale() const
const Pose2 & GetCorrectedPose() const