Occupancy Grid Mapping (in JS)

grid_test.js

var canvas_actual = document.getElementById('canvas_actual');
var canvas_occ = document.getElementById('canvas_occupancy');
var ctx_actual = canvas_actual.getContext('2d');
var ctx_occ = canvas_occ.getContext('2d');

var map_scale = 10; // cm per grid square side
var beam_dist_grid = 400; // 400 grid squares long
var beam_dist_actual = beam_dist_grid * map_scale; // ... in cm
var base_point = {x:300, y:300}

/* The specified accuracy for the LidarLite-2v3 is +/- 2.5cm-- I'm taking that
 * as the 2-sigma value, so sigma is 2.5/2.
 */
var sensor_real_stddev = (10 / 2);

/* The absolute encoder jumps a bit sometimes, randomly increasing or decreasing by 1.
 * So the 2-sigma range is ((1 / 1024) * 2PI) radians; the 2's cancel out,
 * so sigma is just 1 * (PI/1024).
 * In reality, it's probably slightly lower, but this is being conservative.
 */
var angle_real_stddev = (Math.PI / 1024);

var sensor_stddev = sensor_real_stddev;
var angle_stddev = angle_real_stddev;

const width = 600, height = 600;

function random_normal() {
    var u = 0, v = 0;
    while(u === 0) u = Math.random(); //Converting [0,1) to (0,1)
    while(v === 0) v = Math.random();
    return Math.sqrt( -2.0 * Math.log( u ) ) * Math.cos( 2.0 * Math.PI * v );
}

var map_grid = new OccupancyGrid(width, height, map_scale);
var actual_grid = new BinaryGrid(width, height);

/* Setup initial walls with corners at:
 * (50, 50) -- (550, 50)
 * |            |
 * (50, 550) -- (550, 550)
 */
var c0 = {x:50, y:50}, c1 = {x: 550, y: 50}, c2 = {x: 50, y: 550}, c3 = {x:550, y:550};
actual_grid.set_line(c0, c1, true);
actual_grid.set_line(c0, c2, true);
actual_grid.set_line(c3, c1, true);
actual_grid.set_line(c3, c2, true);

var angle_elem = document.getElementById('angle');

function do_map_update(angle_rad) {
    var noisy_angle = (random_normal() * angle_stddev) + angle_rad;
    var raycast_cells = actual_grid.get_raycast_dist(base_point.x, base_point.y, noisy_angle, beam_dist_grid);
    var raycast_actual = raycast_cells * map_scale;
    var raycast_noisy = (random_normal() * sensor_stddev) + raycast_actual;
    //console.log(`Raycast distance: ${raycast_cells} cells, ${raycast_actual} cm`);
    //console.log(`Noisy value: ${raycast_noisy}`);

    map_grid.sensorUpdate(base_point.x, base_point.y, angle_rad, raycast_noisy, beam_dist_actual, sensor_stddev);
}

function render_lines(angle_rad) {
    var dx = Math.cos(angle_rad) * beam_dist_grid;
    var dy = Math.sin(angle_rad) * beam_dist_grid;

    var raycast_cells = actual_grid.get_raycast_dist(base_point.x, base_point.y, angle_rad, beam_dist_grid);
    var rx = Math.cos(angle_rad) * raycast_cells;
    var ry = Math.sin(angle_rad) * raycast_cells;

    var beam_points = cells_on_line(base_point.x, base_point.y, dx, dy, actual_grid.size);
    var raycast_points = cells_on_line(base_point.x, base_point.y, rx, ry, actual_grid.size);

    var actual_img_data = ctx_actual.createImageData(width, height);
    actual_img_data = actual_grid.render(actual_img_data);

    var occ_img_data = ctx_occ.createImageData(width, height);
    occ_img_data = map_grid.render(occ_img_data);

    beam_points.forEach(
        (p) => {
            render_point(actual_img_data, p, actual_grid.size, {r:0, g: 0, b: 255});
        }
    );

    raycast_points.forEach(
        (p) => {
            render_point(actual_img_data, p, actual_grid.size, {r:255, g: 0, b: 0});
            render_point(occ_img_data, p, map_grid.size, {r:255, g: 0, b: 0});
        }
    );

    ctx_actual.putImageData(actual_img_data, 0, 0);
    ctx_occ.putImageData(occ_img_data, 0, 0);
}

function make_block(x,y, radius, state) {
    for(let ix=-radius;ix<=radius;ix++) {
        for(let iy=-radius;iy<=radius;iy++) {
            actual_grid.set_grid_point(x+ix, y+iy, state);
        }
    }
}

var editState = false;
canvas_actual.onmousedown = (event) => {
    var x = event.pageX - canvas_actual.offsetLeft;
    var y = event.pageY - canvas_actual.offsetTop;

    editState = !actual_grid.get_grid_point(x,y);
    if(editState) {
        make_block(x, y, 3, editState);
    } else {
        make_block(x, y, 5, editState);
    }
}

canvas_actual.onmousemove = (event) => {
    var x = event.pageX - canvas_actual.offsetLeft;
    var y = event.pageY - canvas_actual.offsetTop;

    var lmb = (event.buttons & 1);
    if(lmb > 0) {
        if(editState) {
            make_block(x, y, 3, editState);
        } else {
            make_block(x, y, 5, editState);
        }
    }
}

var toggleSweepButton = document.getElementById('toggle-sweep');
var sweepState = false;
toggleSweepButton.textContent = (sweepState ? 'Stop Sweep' : 'Start Sweep');

toggleSweepButton.onclick = (event) => {
    sweepState = !sweepState;
    toggleSweepButton.textContent = (sweepState ? 'Stop Sweep' : 'Start Sweep');
}

/* sensor sweep */
var angle_deg = 0;
var keys = { left: false, right: false, up: false, down: false };

window.onkeydown = (event) => {
    switch(event.key) {
        case 'ArrowDown':
            keys.down = true;
            break;
        case 'ArrowUp':
            keys.up = true;
            break;
        case 'ArrowLeft':
            keys.left = true;
            break;
        case 'ArrowRight':
            keys.right = true;
            break;
    }
}

window.onkeyup = (event) => {
    switch(event.key) {
        case 'ArrowDown':
            keys.down = false;
            break;
        case 'ArrowUp':
            keys.up = false;
            break;
        case 'ArrowLeft':
            keys.left = false;
            break;
        case 'ArrowRight':
            keys.right = false;
            break;
    }
}


window.setInterval(() => {
    var angle_rad = angle_deg * (Math.PI / 180.0);

    if(keys.left) {
        base_point.x = Math.max(base_point.x-5, 0);
    }

    if(keys.right) {
        base_point.x = Math.min(base_point.x+5, actual_grid.size.x);
    }

    if(keys.up) {
        base_point.y = Math.max(base_point.y-5, 0);
    }

    if(keys.down) {
        base_point.y = Math.min(base_point.y+5, actual_grid.size.y);
    }

    if(sweepState) {
        var old_angle = angle_deg;
        angle_deg += 360 / 10;

        for(let i=old_angle; Math.abs(i-angle_deg) >= 0.5; i += 0.25) {
            angle_rad = i * (Math.PI / 180.0);
            do_map_update(angle_rad);
        }

        angle_rad = angle_deg * (Math.PI / 180.0);
    }

    render_lines(angle_rad);
}, 10);


render_lines(0);

occupancy_grid.html

<!DOCTYPE html>
<html>
    <head>
        <meta charset="utf-8">
        <title>Occupancy Grid Test</title>
        <style media="screen">
            body {
                background-color: grey;
            }
        </style>
    </head>
    <body>
        <canvas id="canvas_actual" width="600" height="600"></canvas>
        <canvas id="canvas_occupancy" width="600" height="600"></canvas>
        <button type="button" id="toggle-sweep"></button>
        <script src="occupancy_grid.js" charset="utf-8"></script>
        <script src="grid_test.js" charset="utf-8"></script>
    </body>
</html>

occupancy_grid.js

function euclidean_dist(x0, y0, x1, y1) {
    return Math.sqrt(Math.pow(x0 - x1, 2) + Math.pow(y0 - y1, 2));
}

function point_to_index(point, size) {
    return point.x + (point.y * size.x);
}

function cells_on_line(x0, y0, dx, dy, size) {
    var nx = Math.abs(dx), ny = Math.abs(dy);
    var sx = (dx > 0 ? 1 : -1), sy = (dy > 0 ? 1 : -1);

    var points = [{x: x0, y: y0}];
    var cur_x = x0, cur_y = y0;
    for(let ix=0, iy=0; (ix <= nx) && (iy <= ny);) {
        if(Math.abs( ((0.5 + ix) / nx) - ((0.5 + iy) / ny)) < 0.0000001) {
            cur_x += sx;
            cur_y += sy;
            ix++;
            iy++;
        } else if((0.5 + ix) / nx < (0.5 + iy) / ny) {
            cur_x += sx;
            ix++;
        } else {
            cur_y += sy;
            iy++;
        }

        if(size !== undefined) {
            if(cur_x < 0 || cur_x > size.x) {
                break;
            }

            if(cur_y < 0 || cur_y > size.y) {
                break;
            }
        }

        points.push({ x: cur_x, y: cur_y });
    }

    return points;
}

class BinaryGrid {
    constructor(width, height) {
        this.size = {x: width, y: height};
        this.grid = new Uint8Array(width * height);
    }

    set_grid_point(x, y, state) {
        var idx = point_to_index({x,y}, this.size);
        this.grid[idx] = (state ? 255 : 0);
    }

    get_grid_point(x, y) {
        var idx = point_to_index({x,y}, this.size);
        return this.grid[idx] > 0;
    }

    set_line(p0, p1, state) {
        var points = cells_on_line(p0.x, p0.y, p1.x-p0.x, p1.y-p0.y);

        points.forEach(
            (p) => {
                var idx = point_to_index(p, this.size);
                this.grid[idx] = (state ? 255 : 0);
            }
        );
    }

    get_raycast_dist(x, y, theta, maxDist) {
        var dx = (Math.cos(theta)*maxDist);
        var dy = (Math.sin(theta)*maxDist);

        var points = cells_on_line(x, y, dx, dy, this.size);

        return points.reduce(
            (acc, val) => {
                var idx = point_to_index(val, this.size);
                if(this.grid[idx] === 0) {
                    return acc; // maintain old value
                }

                var dist = euclidean_dist(x, y, val.x, val.y);
                if(dist < acc) {
                    return dist;
                } else {
                    return acc;
                }
            }, maxDist
        );
    }

    render(imageData) {
        for(let y=0;y<this.size.y;y++) {
            for(let x=0;x<this.size.x;x++) {
                var idx = x + (this.size.x * y);
                if(this.grid[idx] > 0) {
                    render_point(imageData, {x,y}, this.size, {r:0, g:0, b:0});
                } else {
                    if(x % 10 === 0 || y % 10 === 0) {
                        render_point(imageData, {x,y}, this.size, {r:255-32, g:255-32, b:255-32});
                    } else {
                        render_point(imageData, {x,y}, this.size, {r:255, g:255, b:255});
                    }
                }
            }
        }

        return imageData;
    }
}


function render_point(imgData, p, size, color) {
    var idx = (p.x * 4) + (size.x * p.y * 4);
    imgData.data[idx] = color.r;
    imgData.data[idx+1] = color.g;
    imgData.data[idx+2] = color.b;
    imgData.data[idx+3] = 255;
}

class OccupancyGrid {
    /* grid scale = width/height of 1 grid square in cm */
    constructor(width, height, gridScale, min_occ, max_occ) {
        this.size = {x: width, y: height};
        this.grid = new Float64Array(width * height);
        this.scale = gridScale;
        this.min_occ = min_occ || Math.log(0.01 / 0.99);
        this.max_occ = max_occ || Math.log(0.99 / 0.01);
    }

    /*
     * negative modifier = lower probability occupied
     * positive modifier = higher probability occupied
     */
    sensorModifier(dist, dist_exp, dist_max, std_dev) {
        var variance = Math.pow(std_dev, 2);

        var max_range_size = 6*std_dev; // twice the three-sigma range

        // log(p_occ)
        var p_occ = -1 * Math.pow(dist - dist_exp, 2) / (2*variance);
        p_occ -= Math.log(Math.sqrt(2*Math.PI*variance));

        var p_free = (1 / dist_max);
        if(Math.abs(dist - dist_max) <= 3*std_dev) {
            // add max-range probability
            p_free += (1/max_range_size);
            p_free -= (1/(max_range_size * dist_max));
        }
        p_free = Math.log(p_free);

        //console.log(`log(p_occ) = ${p_occ}\nlog(p_free) = ${p_free}`);

        return p_occ - p_free;
        /*
        var rand = Math.log(dist_max / Math.sqrt(2*Math.PI*variance));
        var hit = Math.pow(dist - dist_exp, 2) / (2*variance);

        return rand - hit;
        */
    }

    diagonal_dist(theta, dist) {
        return Math.max(Math.abs(Math.cos(theta)), Math.abs(Math.sin(theta)))*dist;
    }

    /* x,y = position updating from
     * theta = direction of sensor beam (0 = +X axis, increases CCW)
     * dist
     * max_dist
     * std_dev = standard deviation of
     */
    sensorUpdate(x, y, theta, dist, max_dist, std_dev) {
        /* dx and dy are in units of grid squares */
        var dx = (Math.cos(theta)*dist) / this.scale;
        var dy = (Math.sin(theta)*dist) / this.scale;

        var points = cells_on_line(x, y, dx, dy, this.size);

        for(let i=0;i<points.length;i++) {
            let cur = points[i];
            let grid_dist = euclidean_dist(cur.x, cur.y, x, y);

            let modifier = this.sensorModifier(
                dist,
                grid_dist * this.scale, /* Go from grid scale to actual scale */
                max_dist,
                std_dev
            );

            let gridIdx = cur.x + (this.size.x * cur.y);
            this.grid[gridIdx] += modifier;

            this.grid[gridIdx] = Math.max(this.grid[gridIdx], this.min_occ);
            this.grid[gridIdx] = Math.min(this.grid[gridIdx], this.max_occ);
        }
    }

    render(imageData) {
        for(let y=0;y<this.size.y;y++) {
            for(let x=0;x<this.size.x;x++) {
                var idx = x + (this.size.x * y);
                if(this.grid[idx] > 0) {
                    render_point(imageData, {x,y}, this.size, {r:0, g:0, b:0});
                } else {
                    if(x % 10 === 0 || y % 10 === 0) {
                        render_point(imageData, {x,y}, this.size, {r:255-32, g:255-32, b:255-32});
                    } else {
                        render_point(imageData, {x,y}, this.size, {r:255, g:255, b:255});
                    }
                }
            }
        }

        return imageData;
    }
}