diff --git a/examples/log2pgm.cpp b/examples/log2pgm.cpp new file mode 100644 index 0000000..5034ab5 --- /dev/null +++ b/examples/log2pgm.cpp @@ -0,0 +1,443 @@ +/* +log2pgm.cpp : BreezySLAM demo. Reads logfile with odometry and scan data from +Paris Mines Tech and produces a .PGM image file showing robot path +and final map. + +For details see + +@inproceedings{, + author = {Bruno Steux and Oussama El Hamzaoui}, + title = {SinglePositionSLAM: a SLAM Algorithm in less than 200 lines of C code}, + booktitle = {11th International Conference on Control, Automation, Robotics and Vision, ICARCV 2010, Singapore, 7-10 + December 2010, Proceedings}, + pages = {1975-1979}, + publisher = {IEEE}, + year = {2010} +} + +Copyright (C) 2014 Simon D. Levy + +This code is free software: you can redistribute it and/or modify +it under the terms of the GNU Lesser General Public License as +published by the Free Software Foundation, either version 3 of the +License, or (at your option) any later version. + +This code is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU Lesser General Public License +along with this code. If not, see . + +Change log: + +20-APR-2014 - Simon D. Levy - Get params from command line +05-JUN-2014 - SDL - get random seed from command line +*/ + +// SinglePositionSLAM params: gives us a nice-size map +static const int MAP_SIZE_PIXELS = 800; +static const double MAP_SIZE_METERS = 32; + +static const int SCAN_SIZE = 682; + +// Arbitrary maximum length of line in input logfile +#define MAXLINE 10000 + +#include +#include +using namespace std; + +#include +#include +#include +#include +#include + +#include "Position.hpp" +#include "Laser.hpp" +#include "WheeledRobot.hpp" +#include "PoseChange.hpp" +#include "algorithms.hpp" + + +// Methods to load all data from file ------------------------------------------ +// Each line in the file has the format: +// +// TIMESTAMP ... Q1 Q1 ... Distances +// (usec) (mm) +// 0 ... 2 3 ... 24 ... +// +//where Q1, Q2 are odometry values + +static void skiptok(char ** cpp) +{ + *cpp = strtok(NULL, " "); +} + +static int nextint(char ** cpp) +{ + skiptok(cpp); + + return atoi(*cpp); +} + +static void load_data( + const char * dataset, + vector & scans, + vector & odometries) +{ + char filename[256]; + + sprintf(filename, "%s.dat", dataset); + printf("Loading data from %s ... \n", filename); + + FILE * fp = fopen(filename, "rt"); + + if (!fp) + { + fprintf(stderr, "Failed to open file\n"); + exit(1); + } + + char s[MAXLINE]; + + while (fgets(s, MAXLINE, fp)) + { + char * cp = strtok(s, " "); + + long * odometry = new long [3]; + odometry[0] = atol(cp); + skiptok(&cp); + odometry[1] = nextint(&cp); + odometry[2] = nextint(&cp); + + odometries.push_back(odometry); + + // Skip unused fields + for (int k=0; k<20; ++k) + { + skiptok(&cp); + } + + int * scanvals = new int [SCAN_SIZE]; + + for (int k=0; kTICKS_PER_CYCLE); + } + +private: + + double ticksToDegrees(double ticks) + { + return ticks * (180. / this->TICKS_PER_CYCLE); + } + + static const int TICKS_PER_CYCLE = 2000; +}; + +// Progress-bar class +// Adapted from http://code.activestate.com/recipes/168639-progress-bar-class/ +// Downloaded 12 January 2014 + +class ProgressBar +{ +public: + + ProgressBar(int minValue, int maxValue, int totalWidth) + { + strcpy(this->progBar, "[]"); // This holds the progress bar string + this->min = minValue; + this->max = maxValue; + this->span = maxValue - minValue; + this->width = totalWidth; + this->amount = 0; // When amount == max, we are 100% done + this->updateAmount(0); // Build progress bar string + } + + void updateAmount(int newAmount) + { + if (newAmount < this->min) + { + newAmount = this->min; + } + if (newAmount > this->max) + { + newAmount = this->max; + } + + this->amount = newAmount; + + // Figure out the new percent done, round to an integer + float diffFromMin = float(this->amount - this->min); + int percentDone = (int)round((diffFromMin / float(this->span)) * 100.0); + + // Figure out how many hash bars the percentage should be + int allFull = this->width - 2; + int numHashes = (int)round((percentDone / 100.0) * allFull); + + + // Build a progress bar with hashes and spaces + strcpy(this->progBar, "["); + this->addToProgBar("#", numHashes); + this->addToProgBar(" ", allFull-numHashes); + strcat(this->progBar, "]"); + + // Figure out where to put the percentage, roughly centered + int percentPlace = (strlen(this->progBar) / 2) - ((int)(log10(percentDone+1)) + 1); + char percentString[5]; + sprintf(percentString, "%d%%", percentDone); + + // Put it there + for (int k=0; kprogBar[percentPlace+k] = percentString[k]; + } + + } + + char * str() + { + return this->progBar; + } + +private: + + char progBar[1000]; // more than we should ever need + int min; + int max; + int span; + int width; + int amount; + + void addToProgBar(const char * s, int n) + { + for (int k=0; kprogBar, s); + } + } +}; + +// Helpers ---------------------------------------------------------------- + +int coords2index(double x, double y) +{ + return y * MAP_SIZE_PIXELS + x; +} + + +int mm2pix(double mm) +{ + return (int)(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS)); +} + +int main( int argc, const char** argv ) +{ + // Bozo filter for input args + if (argc < 3) + { + fprintf(stderr, + "Usage: %s \n", + argv[0]); + fprintf(stderr, "Example: %s exp2 1 9999\n", argv[0]); + exit(1); + } + + // Grab input args + const char * dataset = argv[1]; + bool use_odometry = atoi(argv[2]) ? true : false; + int random_seed = argc > 3 ? atoi(argv[3]) : 0; + + // Load the Lidar and odometry data from the file + vector scans; + vector odometries; + load_data(dataset, scans, odometries); + + // Build a robot model in case we want odometry + Rover robot = Rover(); + + // Create a byte array to receive the computed maps + unsigned char * mapbytes = new unsigned char[MAP_SIZE_PIXELS * MAP_SIZE_PIXELS]; + + // Create SLAM object + MinesURG04LX laser; + SinglePositionSLAM * slam = random_seed ? + (SinglePositionSLAM*)new RMHC_SLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed) : + (SinglePositionSLAM*)new Deterministic_SLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS); + + // Report what we're doing + int nscans = scans.size(); + printf("Processing %d scans with%s odometry / with%s particle filter...\n", + nscans, use_odometry ? "" : "out", random_seed ? "" : "out"); + ProgressBar * progbar = new ProgressBar(0, nscans, 80); + + // Start with an empty trajectory of positions + vector trajectory; + + // Start timing + time_t start_sec = time(NULL); + + // Loop over scans + for (int scanno=0; scannoupdate(lidar, poseChange); + } + else + { + slam->update(lidar); + } + + Position position = slam->getpos(); + + // Add new coordinates to trajectory + double * v = new double[2]; + v[0] = position.x_mm; + v[1] = position.y_mm; + trajectory.push_back(v); + + // Tame impatience + progbar->updateAmount(scanno); + printf("\r%s", progbar->str()); + fflush(stdout); + } + + // Report speed + time_t elapsed_sec = time(NULL) - start_sec; + printf("\n%d scans in %ld seconds = %f scans / sec\n", + nscans, elapsed_sec, (float)nscans/elapsed_sec); + + // Get final map + slam->getmap(mapbytes); + + // Put trajectory into map as black pixels + for (int k=0; k<(int)trajectory.size(); ++k) + { + double * v = trajectory[k]; + + int x = mm2pix(v[0]); + int y = mm2pix(v[1]); + + delete v; + + mapbytes[coords2index(x, y)] = 0; + } + + // Save map and trajectory as PGM file + + char filename[100]; + sprintf(filename, "%s.pgm", dataset); + printf("\nSaving map to file %s\n", filename); + + FILE * output = fopen(filename, "wt"); + + fprintf(output, "P2\n%d %d 255\n", MAP_SIZE_PIXELS, MAP_SIZE_PIXELS); + + for (int y=0; y