diff_generation.py

Go to the documentation of this file.

# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Bhaskara Marthi

# Generation of diffs between two world models

import mongo_ros as mr
import logging
import semanticmodel.msg as msg
import numpy as np
import semanticmodel.plane_data_association as pd
import semanticmodel.db as sdb
import math

log = logging.getLogger('world_model.diffs')
TOL = 0.1

def generate_diffs(run1, run2, db):
    """
    Generate diffs between run1 and run2, which must have been
    produced by the blob storage system.

    @type run1: string
    @type run2: string
    @param db: the name of the overall database
    """
    log.debug("Getting diffs between {0} and {1}".format(run1, run2))
    matches = []
    nonmatches = []
    for run in (run1, run2):
        other = run1 if run is run2 else run2
        log.debug("Processing {0}, and looking for matches in {1}".
                  format(run, other))
        c1 = mr.MessageCollection(db, sdb.collection_name(run, 'blobs'),
                                  msg.BlobMessage)
        c2 = mr.MessageCollection(db, sdb.collection_name(other, 'blobs'),
                                  msg.BlobMessage)
        for cluster, meta in c1.query({}):
            log.debug("Considering cluster {0[id]} at {0[x]}, {0[y]}".
                    format(meta))
            match = find_matching_cluster(cluster, meta, c2)
            if match and run is run1:
                log.debug("  Found match {0[id]} at {0[x]}, {0[y]}".
                        format(match[1]))
                matches.append((meta['id'], match[1]['id']))
            else:
                nearby = next(nearby_clusters(meta, c2), None)
                if nearby and run is run1:
                    nonmatches.append((meta['id'], nearby[1]['id']))
                
    return {'matches': matches, 'nonmatches': nonmatches}

def nearby_clusters(m, c2):
    x = m['x']
    y = m['y']
    return c2.query({'x': {'$gt': x-1.0, '$lt': x+1.0}, 
        'y': {'$gt': y-1.0, '$lt': y+1.0}})


def find_matching_cluster(cluster, meta, c2):
    possible_matches = nearby_clusters(meta, c2) 
    hull = get_hull_polygon(cluster)

    # Check if hulls overlap
    for m in possible_matches:
        if pd.polygons_intersect(hull, get_hull_polygon(m[0])):
            return m

    return None


def get_hull_polygon(cluster):
    return np.array([(p.x,p.y) for p in cluster.hull])


def dist(p, q):
    return math.sqrt(pow(q[0]-p[0],2) + pow(q[1]-p[1],2))


def nearest_nonmatch(cluster, img_coll, max_distance=2.0, max_angle=1.0):
    """
    A crude and inefficient way to find the nearest nonmatching image.  
    """
    loc = (cluster['x'], cluster['y'])
    for img in img_coll.query({}, True):
        vp = (img['x'], img['y'])
        if dist(vp, loc) < max_distance:
            disp = (loc[0]-vp[0], loc[1]-vp[1])
            angle = math.atan2(disp[1], disp[0])
            if (abs(angle-img['theta'])<max_angle):
                return img