From 291035b14cfa6a9f3432e19c5e69b2dbdbcbb582 Mon Sep 17 00:00:00 2001
From: "Simon D. Levy" <simon.d.levy@gmail.com>
Date: Sun, 26 Oct 2014 18:06:15 -0400
Subject: [PATCH] Create Log2PGM.java

---
 examples/Log2PGM.java | 363 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 363 insertions(+)
 create mode 100644 examples/Log2PGM.java

diff --git a/examples/Log2PGM.java b/examples/Log2PGM.java
new file mode 100644
index 0000000..35834d0
--- /dev/null
+++ b/examples/Log2PGM.java
@@ -0,0 +1,363 @@
+/*
+   Log2PGM.java : 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 <http://www.gnu.org/licenses/>.
+ */
+
+import edu.wlu.cs.levy.breezyslam.components.Map;
+import edu.wlu.cs.levy.breezyslam.components.Scan;
+import edu.wlu.cs.levy.breezyslam.components.Position;
+import edu.wlu.cs.levy.breezyslam.components.URG04LX;
+import edu.wlu.cs.levy.breezyslam.components.Velocities;
+
+import edu.wlu.cs.levy.breezyslam.robots.WheeledRobot;
+
+import edu.wlu.cs.levy.breezyslam.algorithms.RMHCSLAM;
+import edu.wlu.cs.levy.breezyslam.algorithms.DeterministicSLAM;
+import edu.wlu.cs.levy.breezyslam.algorithms.SinglePositionSLAM;
+
+import java.io.BufferedReader;
+import java.io.FileReader;
+import java.io.BufferedWriter;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.Vector;
+
+public class Log2PGM
+{
+    private static int    MAP_SIZE_PIXELS = 800;
+    private static double MAP_SIZE_METERS = 32;
+    private static int    SCAN_SIZE       = 682;
+
+    private SinglePositionSLAM slam;
+
+    private boolean use_odometry;
+    private int random_seed;
+    private Rover robot;
+
+    static int coords2index(double x,  double y)
+    {    
+        return (int)(y * MAP_SIZE_PIXELS + x);
+    }
+
+    int mm2pix(double mm)
+    {
+        return (int)(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS));  
+    }
+
+
+    // Class for Mines verison of URG-04LX Lidar -----------------------------------
+
+    private class MinesURG04LX extends URG04LX
+    {
+        public MinesURG04LX() 
+        {
+            super( 70,          // detectionMargin
+                    145);         // offsetMillimeters
+        }
+    }
+
+    // Class for MinesRover custom robot -------------------------------------------
+
+    private class Rover extends WheeledRobot
+    {
+        public Rover() 
+        {
+            super(77,       // wheelRadiusMillimeters
+                    165);     // halfAxleLengthMillimeters
+        }    
+
+        protected WheeledRobot.WheelOdometry extractOdometry(
+                double timestamp, 
+                double leftWheelOdometry, 
+                double rightWheelOdometry)
+        {        
+            // Convert microseconds to seconds, ticks to angles        
+            return new WheeledRobot.WheelOdometry(
+                    timestamp / 1e6, 
+                    ticksToDegrees(leftWheelOdometry),
+                    ticksToDegrees(rightWheelOdometry));
+        }
+
+        protected String descriptorString()
+        {
+            return String.format("ticks_per_cycle=%d", this.TICKS_PER_CYCLE);
+        }
+
+
+        private double ticksToDegrees(double ticks)
+        {
+            return ticks * (180. / this.TICKS_PER_CYCLE);
+        }
+
+        private int TICKS_PER_CYCLE = 2000;
+    }
+
+    // Progress-bar class ----------------------------------------------------------------
+    // Adapted from http://code.activestate.com/recipes/168639-progress-bar-class/
+    // Downloaded 12 January 2014
+
+    private class ProgressBar
+    {
+        public ProgressBar(int minValue, int maxValue, int totalWidth)
+        {
+            this.progBar = "[]";
+            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)java.lang.Math.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)java.lang.Math.round((percentDone / 100.0) * allFull);
+
+            // Build a progress bar with hashes and spaces
+            this.progBar = "[";
+            this.addToProgBar("#", numHashes);
+            this.addToProgBar(" ", allFull-numHashes);
+            this.progBar += "]";
+
+            // Figure out where to put the percentage, roughly centered
+            int percentPlace =  (this.progBar.length() / 2) - ((int)(java.lang.Math.log10(percentDone+1)) + 1);
+            String percentString = String.format("%d%%", percentDone);
+
+            // Put it there
+            this.progBar = this.progBar.substring(0,percentPlace) + percentString + this.progBar.substring(percentPlace+percentString.length());
+        }
+
+        String str()
+        {   
+            return this.progBar;
+        }
+
+        private String progBar; 
+        private int min;
+        private int max;
+        private int span;
+        private int width;
+        private int amount;
+
+        private void addToProgBar(String s, int n)
+        {
+            for (int k=0; k<n; ++k)
+            {
+                this.progBar += s;
+            }
+        }
+    }
+
+    public Log2PGM(boolean use_odometry, int random_seed)
+    {
+        MinesURG04LX laser = new MinesURG04LX();
+
+        this.robot = new Rover(); 
+
+        this.slam = random_seed > 0 ? 
+            new RMHCSLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed) :
+            new DeterministicSLAM(laser, MAP_SIZE_PIXELS, MAP_SIZE_METERS);
+
+        this.use_odometry = use_odometry;
+        this.random_seed = random_seed;
+    }
+
+    public byte [] processData(Vector<int []> scans, Vector<long []> odometries)
+    {
+        // Report what we're doing
+        int nscans = scans.size();
+        ProgressBar  progbar = new ProgressBar(0, nscans, 80); 
+
+        System.out.printf("Processing %d scans with%s odometry / with%s particle filter...\n",
+                nscans, this.use_odometry ? "" : "out", this.random_seed > 0 ? "" : "out");
+
+        Vector<double []> trajectory = new Vector<double []>();
+
+        for (int scanno=0; scanno<nscans; ++scanno)
+        {
+            int [] scan = scans.elementAt(scanno);
+
+            // Update with/out odometry
+            if (this.use_odometry)
+            {
+                long [] odometry = odometries.elementAt(scanno);
+                Velocities velocities = this.robot.computeVelocities(odometry[0], odometry[1], odometry[2]);
+                slam.update(scan, velocities);            
+            }
+            else
+            {
+                slam.update(scan);  
+            }
+
+            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.add(v);     
+
+ 
+            progbar.updateAmount(scanno);
+            System.out.printf("\r%s", progbar.str());
+        } 
+
+        // Get final map
+        byte [] mapbytes = new byte [MAP_SIZE_PIXELS*MAP_SIZE_PIXELS];
+        slam.getmap(mapbytes);
+
+        // Put trajectory into map as black pixels
+        for (int k=0; k<trajectory.size(); ++k)
+        {        
+            double [] v = trajectory.elementAt(k);
+                            
+            int x = mm2pix(v[0]);
+            int y = mm2pix(v[1]);
+            
+            mapbytes[coords2index(x, y)] = 0;
+        }
+     
+        return mapbytes;
+    }
+
+    // main -------------------------------------------------------------------------------------------------------------
+
+    public static void main(String[] argv)
+    {
+        // Bozo filter for input args
+        if (argv.length < 3)
+        {
+            System.err.println("Usage:   java log2pgm <dataset> <use_odometry> <random_seed>");
+            System.err.println("Example: java log2pgm exp2 1 9999");
+            System.exit(1);
+        }
+
+        // Grab input args
+        String dataset = argv[0];
+        boolean use_odometry =  Integer.parseInt(argv[1])  == 0 ? false : true;
+        int random_seed =  argv.length > 2 ? Integer.parseInt(argv[2]) : 0;
+
+        // Load the data from the file    
+
+        Vector<int []>  scans = new Vector<int []>();
+        Vector<long []>  odometries = new Vector<long []>();;
+
+        String filename = dataset + ".dat";
+        System.out.printf("Loading data from %s ... \n", filename);
+
+        BufferedReader input = null;
+
+        try  
+        {
+            FileReader fstream = new FileReader(filename);
+            input = new BufferedReader(fstream);
+            while (true)
+            {
+                String line = input.readLine();
+                if (line == null) 
+                {
+                    break;
+                }
+
+                String [] toks = line.split(" +");
+
+                long [] odometry = new long [3];
+                odometry[0] = Long.parseLong(toks[0]);
+                odometry[1] = Long.parseLong(toks[2]);
+                odometry[2] = Long.parseLong(toks[3]);
+                odometries.add(odometry);
+
+                int [] scan = new int [SCAN_SIZE];
+                for (int k=0; k<SCAN_SIZE; ++k)
+                {
+                    scan[k] = Integer.parseInt(toks[k+24]);
+                }
+                scans.add(scan);
+            }
+            input.close();
+        }
+        catch (IOException e)
+        {
+            System.err.println("Error: " + e.getMessage());
+        }
+
+        // Create a Log2PGM object to process the data
+        Log2PGM log2pgm = new Log2PGM(use_odometry, random_seed);
+
+        // Process the scan and odometry data, returning a map
+        byte [] mapbytes = log2pgm.processData(scans, odometries);
+
+        // Save map and trajectory as PGM file    
+
+        filename = dataset + ".pgm";
+        System.out.println("\nSaving map to file " + filename);
+
+        BufferedWriter output = null;
+
+        try  
+        {
+            FileWriter fstream = new FileWriter(filename);
+            output = new BufferedWriter(fstream);
+            output.write(String.format("P2\n%d %d 255\n", MAP_SIZE_PIXELS, MAP_SIZE_PIXELS));
+            for (int y=0; y<MAP_SIZE_PIXELS; y++)
+            {
+                for (int x=0; x<MAP_SIZE_PIXELS; x++)
+                {
+                    // Output unsigned byte value
+                    output.write(String.format("%d ", (int)mapbytes[coords2index(x, y)] & 0xFF));
+                }
+                output.write("\n");
+            }
+            output.close();
+        }
+        catch (IOException e)
+        {
+            System.err.println("Error: " + e.getMessage());
+        }
+    }
+}