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