plane_data_association.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

# Code for data association and registration of planes across frames

import scipy as sp
import numpy as np
import numpy.linalg as lin
import semanticmodel.msg as sm
import geometry_msgs.msg as gm
import visualization_msgs.msg as vm
from rospy import loginfo, logdebug
import copy
from std_msgs.msg import ColorRGBA
from random import random
import convexhull as ch
import logging

LOG=logging.getLogger('world_model.planes')

DEFAULT_MAX_DISP=0.2
COLORS = []

class BoundedPlane:
    "Represents a polygonal subset of a plane in 3-space"

    def __init__(self, p):
        "Init from a Plane message"

        self.header = p.header

        workaround_incorrect_planes(p)
        
        # Normalize the plane equation
        self.n, self.d = normalize_eq(p)

        # Figure out transforms between plane and world coordinates
        assert(len(p.polygon)>2)
        p0 = p.polygon[0]
        p1 = p.polygon[1]
        p0 = np.array([p0.x, p0.y, p0.z])
        p1 = np.array([p1.x, p1.y, p1.z])
        v1 = normalize(p1-p0) # First basis vector is along polygon side
        v2 = normalize(np.cross(self.n, v1)) # Orthogonal basis vector in plane
        h1 = np.hstack([v1,0])
        h2 = np.hstack([v2,0])
        h3 = np.hstack([self.n,0])
        h4 = np.hstack([p0,1])


        # Store transform from plane coords to world as homogeneous matrix
        self.p2w = np.vstack([h1,h2,h3,h4]).T

        # Store transform from world to plane
        self.w2p = lin.inv(self.p2w)

        # Transform bounding polygon into planar coordinates
        self.polygon = [mult(self.w2p, p)[0:2] for p in p.polygon]


    def __repr__(self):
        return "n=[{0}, {1}, {2}], d={3}".\
                   format(self.n[0], self.n[1], self.n[2], self.d)# [to_tuple(p) for p in self.polygon])


def intersect(p1, p2, max_disp=DEFAULT_MAX_DISP):
    """
    Checks if:
    1) the two sets lie on the same plane (approximately)
    2) the polygons intersect
    """
    #loginfo("Checking if {0} and {1} intersect".format(p1, p2))
    trans = np.dot(p1.w2p, p2.p2w)
    poly2 = [trans2d(trans, p)[0:3] for p in p2.polygon]

    # Check that all the points approximately lie on the plane
    for i in range(len(poly2)):
        if abs(poly2[i][2])>max_disp:
            return False
    else:
        poly2 = [p[0:2] for p in poly2]
        res = polygons_intersect(p1.polygon, poly2)
        return res

def merge_planes(planes, p, max_disp=DEFAULT_MAX_DISP):
    """
    P must intersect at least one plane.  Combine it with that plane, then
    process any additional intersections that result.
    """
    planes = planes.copy()
    int_planes = [id for id, existing in planes.items()
                  if intersect(p, existing, max_disp)]
    assert int_planes
    i = int_planes[0]
    LOG.debug("Merging new plane with {0}".format(i))
    planes[i] = combine(planes[i], p)

    merged = True
    while merged:
        merged = False
        LOG.debug("Checking for additional intersections")
        for j, pl in planes.items():
            LOG.debug("Checking {0}".format(j))
            if j != i:
                if intersect(planes[i], planes[j], max_disp):
                    planes[i] = combine(planes[i], planes[j])
                    del planes[j]
                    merged = True
                    LOG.debug("Merged with {0}".format(j))
                    break
    return planes


def combine(p1, p2):
    """
    Combines the two bounded planes to form a new one
    """
    combined = copy.deepcopy(p1)
    poly = [to_tuple(p) for p in p1.polygon]
    trans = np.dot(p1.w2p, p2.p2w)
    for p in p2.polygon:
        poly.append(to_tuple(trans2d(trans, p)))
    hull = ch.convex_hull(poly)
    combined.polygon = map(np.array, hull)
    #logdebug("Combined {0} and {1}.  New poly is {2}".\
    #        format([to_tuple(p) for p in p1.polygon],
    #               [to_tuple(p) for p in p2.polygon],
    #               list(hull)))
    return combined

def to_plane_msg(bp):
    "Turn a BoundedPlane object into a Plane message"
    p = sm.Plane()
    p.header = bp.header
    p.a, p.b, p.c = bp.n
    p.d = bp.d
    p.polygon = [trans_to_pt(bp.p2w, pt) for pt in bp.polygon]
    sx = sum((pt.x for pt in p.polygon))
    sy = sum((pt.y for pt in p.polygon))
    sz = sum((pt.z for pt in p.polygon))
    np = len(p.polygon)
    p.center.x = sx/np
    p.center.y = sy/np
    p.center.z = sz/np
    return p

def to_marker(planes, timestamp=None):
    "Convert list of bounded planes to visualization markers"
    assert(len(planes)>0)
    marker = vm.Marker()
    marker.header.frame_id = planes[0].header.frame_id
    if timestamp:
        marker.header.stamp = timestamp
    marker.ns = 'planes'
    marker.type = vm.Marker.LINE_LIST
    marker.action = vm.Marker.ADD
    marker.scale.x = 0.01
    marker.color.a = 1.0
    marker.color.b = 1.0
    marker.color.r = 0.75

    while len(COLORS)<=len(planes):
        COLORS.append(ColorRGBA(a=1.0, r=random(), g=random(), b=random()))

    for color,bp in zip(COLORS, planes):
        n = len(bp.polygon)
        for i in range(n):
            j = i-1 if i>0 else n-1
            marker.points.append(trans_to_pt(bp.p2w, bp.polygon[j]))
            marker.points.append(trans_to_pt(bp.p2w, bp.polygon[i]))
            marker.colors.append(color)
            marker.colors.append(color)

    return marker
    

############################################################
# Internal
############################################################

def to_tuple(p):
    "Needed to convert between numpy and opencv"
    return (p[0], p[1])

def workaround_incorrect_planes(m):
    n = len(m.polygon)
    new_d = 0
    for pt in m.polygon:
        inc = (m.a*pt.x + m.b*pt.y + m.c*pt.z)
        new_d += inc/n
    m.d = new_d
   


def mult(m, p):
    "Multiply numpy homogeneous matrix * geometry_msgs.Point"
    return np.dot(m, [p.x, p.y, p.z, 1])

def trans2d(m, p):
    "Transform 2d pt using homogeneous matrix"
    return np.dot(m, np.hstack([p, 0, 1]))[0:3]

def trans_to_pt(m, p):
    "Transform 2d pt using m and convert to geometry_msgs.Point"
    return gm.Point(*trans2d(m, p))

def normalize_eq(m):
    v = np.array([m.a, m.b, m.c])
    return normalize(v), m.d/lin.norm(v)

def normalize(v):
    return v/lin.norm(v)

def polygons_intersect(p1, p2):
    return one_sided_intersect(p1, p2) or one_sided_intersect(p2, p1)

def one_sided_intersect(p1, p2):
    """
    Iterate over sides of P1 and see if one of them separates the two polygons.
    If there is, the polygons don't intersect.
    """
    n = len(p1)
    for i in range(n):
        j = i-1 if i>0 else n-1
        min1, max1 = project_onto_normal(p1, p1[i], p1[j])
        min2, max2 = project_onto_normal(p2, p1[i], p1[j])
        if max2<min1 or max1<min2:
            return False
    return True

def project_onto_normal(poly, q1, q2):
    "Project points of poly onto normal to line between q1 and q2"
    d = q2-q1
    perp = normalize(np.array((d[1], -d[0])))
    inf = None
    sup = None
    for p in poly:
        proj = np.dot(perp, p)
        if inf is None or proj<inf:
            inf = proj
        if sup is None or proj>sup:
            sup = proj
    assert inf is not None
    assert sup is not None
    return (inf, sup)



############################################################
# Examples
############################################################

# These defs don't work any more
#
# 2 intersects 3 and 4
# 3 and 4 don't intersect
#
# m = []
# m.append(sm.Plane())
# m[0].a = 1
# m[0].b = 0
# m[0].c = -1
# m[0].d = 4
# m[0].hull.append(gm.Point(1,2,-3))
# m[0].hull.append(gm.Point(6,4,2))
# m[0].hull.append(gm.Point(3,-1,4))

# m.append(sm.Plane())
# m[1].c = 1
# m[1].d = 3
# m[1].hull.append(gm.Point(1,2,3))
# m[1].hull.append(gm.Point(4,5,3))
# m[1].hull.append(gm.Point(4,0,3))

# m.append(sm.Plane())
# m[2].c = 2
# m[2].d = 6
# m[2].hull.append(gm.Point(3,2,3))
# m[2].hull.append(gm.Point(3,8,3))
# m[2].hull.append(gm.Point(6,8,3))
# m[2].hull.append(gm.Point(6,2,3))

# m.append(sm.Plane())
# m[3].c = 1
# m[3].d = 3
# m[3].hull.append(gm.Point(2,2,3))
# m[3].hull.append(gm.Point(2,6,3))
# m[3].hull.append(gm.Point(-1,6,3))
# m[3].hull.append(gm.Point(-1,2,3))


# for msg in m:
#     msg.header.frame_id = '/map'