def PublishSpace(self, bb, sensor,algorithm, aux_payload):
planes = self.GenPlanes(bb)
if len(planes) == 0:
print "WARNING: Camera2BoundingBox:PublishSpace(): Returning None because len(planes) == 0"
return None
space = Convex_Space()
space.planes = planes
space.sensor = sensor
space.algorithm = algorithm
space.aux_payload = aux_payload
self.publisher.publish(space)
# some callers might be interested in seeing what I just published
return space
def init():
output_name = "intersector_input"
publisher = rospy.Publisher(output_name, Convex_Space)
rospy.init_node('intersector')
print "Sleep for 2 seconds to allow the 3d_intersector time to initialize..."
rospy.Rate(.5).sleep()
print "Sleep done..."
while True:
# this is a horizontal slicer
space = Convex_Space()
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = 1
plane.d = -1
space.planes.append(plane);
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = -1
plane.d = 0
space.planes.append(plane);
space.sensor = "horizontal_slicer"
space.algorithm = "horizontal_slicer"
space.aux_payload = "slicer:horizontal"
publisher.publish(space)
rospy.Rate(1).sleep()
def PublishCroppedPyramid(self, polygonStamped, front):
# PERFORMANCE WARNING: I'm guessing that calling transformPoint()
# over and over is going to have to lots of painful lookups, and
# performance will suck. But for now, I'm focusing on correctness
# rather than speed, so I'm calling their API rather than getting
# the transform for myself with lookupTransform().
self.tf_name = polygonStamped.header.frame_id
points = polygonStamped.polygon.points
# abort (with warning) if we don't yet know the position of the
# camera.
if not self.tf_listener.canTransform(self.tf_name, "/map", polygonStamped.header.stamp):
print "WARNING: Could not find the tf for the camera %s" % self.tf_name
return
# note that the tf coordinate system is right handed, with:
# X forward
# Y left
# Z up
# ick.
#
# BTW, I'm assuming that the picture coordinate system is:
# X right
# Y down
# (0,0) at upper-left
# which is also icky, but it's a familiar sort of icky.
# after normalization, left & right give the Y value of those edges
# of the bounding box, according to the tf convention
# after normalization, top & bottom give the Z value of those edges
# of the bounding box, according to the tf convention
# to construct each plane, we build three points in the camera's
# coordinate system, which form a triangle. If you view the
# triangle and see the points in a counter-clockwise direction,
# then you are "in front of" the plane. You would see them in
# clockwise direction if you were in back of it.
#
# remember that the planes face inwards.
# this is the object which will collect the 4 (or maybe 5)
# planes we generate.
poly = Convex_Space()
poly.sensor = self.tf_name
poly.algorithm = self.tf_name
marker.points = []
origin = Point(0,0,0)
polygonStamped.polygon.points = []
polygonStamped.polygon.points.append( self.ConvertToWorldPoint(origin) )
# Gonna use the intrinsic camera matrix K to reverse project
K = self.K_dict[self.tf_name]
for i in range(len(points)):
# print "OLD POINT: %s" % points[i]
transformed = K.I*[[points[i].x], [points[i].y], [1]]
# print "TRANSFORMED: %s" % transformed
""" Camera thinks it's facing down the x-axis, while tf thinks it's
facing down the z, so we put in (z, -x, y)
y also has to be negated here, not sure why??? """
polygonStamped.polygon.points.append( self.ConvertToWorldPoint( Point(transformed[2,0], -transformed[0,0], -transformed[1,0]) ))
return polygonStamped
# Old method
# points[i].x = ( 1 - (2*points[i].x/self.xWid) * self.aspectXtoZ )
# points[i].y = ( 1 - (2*points[i].y/self.yHei) * self.aspectYtoZ )
""" This should add the points for each plane in the correct order,
counter-clockwise if looking from within the pyramid """
origin = Point(0,0,0)
for point1,point2 in zip(points, points[1:]+[points[0]]):
poly.planes.append( self.GenPlane(origin, point2, point1) )
# finally, if front is positive, then we want to produce a bounding
# plane there, facing away from the camera. So, if we stand at the
# origin, it should be a clockwise pattern.
if front > 0:
point1 = Point(front, 0,0)
point2 = Point(front, 1,0)
point2 = Point(front, 1,1)
poly.planes.append( self.GenPlane(point1, point2, point3) )
return poly
#DEAD FOR NOW
# we are at last ready to speak to the world!
self.bb_publisher.publish(poly)
""" This bit is for the purpose of testing the intersector
It publishes a cube that should intersect with the
bounding pyramid generated for the blue_block
"""
poly.sensor = "testing"
poly.algorithm = "testing"
poly.planes = []
"""
# Front and Back planes
poly.planes.append( self.GenPlaneFromWorldPoints(Point(2,0,0),Point(2,1,0),Point(2,0,1)) )
poly.planes.append( self.GenPlaneFromWorldPoints(Point(1,0,0),Point(1,0,1),Point(1,1,0)) )
# Right and Left planes
poly.planes.append( self.GenPlaneFromWorldPoints(Point(0,-2,0),Point(1,-2,0),Point(0,-2,1)) )
poly.planes.append( self.GenPlaneFromWorldPoints(Point(0,-1,0),Point(0,-1,1),Point(1,-1,0)) )
"""
# Top and Bottom planes
poly.planes.append( self.GenPlaneFromWorldPoints(Point(0,0,1),Point(1,0,1),Point(0,1,1)) )
poly.planes.append( self.GenPlaneFromWorldPoints(Point(0,0,0),Point(0,1,0),Point(1,0,0)) )
self.bb_publisher.publish(poly)
self.marker_pub.publish(marker)
def extractSpace(self):
# this extracts a Convex_Space object from this intersection object.
retval = Convex_Space()
retval.planes = self.planes # TODO: cull planes, someday
retval.sensor = "intersector"
retval.algorithm = "intersector"
# the aux_payload of the new Convex_Space is the union of all the
# input payloads, plus the point list and bounding box we have
# calculated.
retval.aux_payload = ""
for space in self.convex_spaces:
# print "AUX PAYLOAD: " + space.aux_payload
retval.aux_payload += space.aux_payload + "\n"
# print
# print retval.aux_payload
# print
# this little loop lists the points we have calculated...
min_x = max_x = self.points[0].x
min_y = max_y = self.points[0].y
min_z = max_z = self.points[0].z
pts_payload = ""
global marker # added by Jeremy
for pt in self.points:
pts_payload += " (%f,%f,%f)" % (pt.x,pt.y,pt.z)
marker.points.append(Point(pt.x,pt.y,pt.z)) # added by Jeremy
if pt.x < min_x:
min_x = pt.x
if pt.x > max_x:
max_x = pt.x
if pt.y < min_y:
min_y = pt.y
if pt.y > max_y:
max_y = pt.y
if pt.z < min_z:
min_z = pt.z
if pt.z > max_z:
max_z = pt.z
bb_payload = "x=(%f,%f) y=(%f,%f) z=(%f,%f)" % (min_x,max_x, min_y,max_y, min_z,max_z)
""" # added by Jeremy - show only bounding box
marker.points.append(Point(min_x,min_y,min_z))
marker.points.append(Point(min_x,min_y,max_z))
marker.points.append(Point(min_x,max_y,max_z))
marker.points.append(Point(max_x,max_y,max_z))
marker.points.append(Point(max_x,max_y,min_z))
marker.points.append(Point(max_x,min_y,min_z))
marker.points.append(Point(max_x,min_y,max_z))
marker.points.append(Point(min_x,max_y,min_z))
"""
retval.aux_payload += "convex_set: %s\n" % pts_payload
retval.aux_payload += "bounding_box: %s\n" % bb_payload
# also, add a count of the input spaces as a quick estimate of how
# interesting this object is.
retval.aux_payload += "convex_space_count: %d" % len(self.convex_spaces)
return retval
def GenPlaneFromWorldPoints(self, point1, point2, point3):
if (point1.x == point2.x and point1.y == point2.y and point1.z == point2.z) or (point1.x == point3.x and point1.y == point3.y and point1.z == point3.z) or (point2.x == point3.x and point2.y == point3.y and point2.z == point3.z):
print "camera2boundingBox.pl:GenPlaneFromWorldPoints(): ERROR: 2 points were identical!!! point1=%s point2=%s point3=%s" % (point1,point2,point3)
return None
AB = [(point2.x-point1.x),(point2.y-point1.y),(point2.z-point1.z)]
AC = [(point3.x-point1.x),(point3.y-point1.y),(point3.z-point1.z)]
normal = cross(AB,AC)
# the a,b,c fields of the plane are the plane normal. d is the
# (negation of the) dot product of the normal times any point on the plane
d = - dot(normal, [point1.x, point1.y, point1.z])
print "GenPlaneFromWorldPoints: point1=(%f,%f,%f) point2=(%f,%f,%f) point3=(%f,%f,%f)" % (point1.x,point1.y,point1.z, point2.x,point2.y,point2.z, point3.x,point3.y,point3.z)
print "GenPlaneFromWorldPoints: normal=(%f,%f,%f)" % (normal[0],normal[1],normal[2])
print "GenPlaneFromWorldPoints: d=%f" % d
print
# BUGFIX: the planes generated were pointing the wrong direction. Multiply
# all values by -1.
# return Plane(normal[0], normal[1], normal[2], d)
return Plane(-normal[0], -normal[1], -normal[2], -d)
# DEAD CODE from here to the end of the class
# this is the object which will collect the 4 (or maybe 5)
# planes we generate.
poly = Convex_Space()
poly.sensor = self.tf_name
poly.algorithm = self.tf_name
# marker.points = []
# Gonna use the intrinsic camera matrix K to reverse project
K = self.K_dict[self.tf_name]
for i in range(len(points)):
transformed = K.I*[[points[i].x], [points[i].y], [1]]
""" Camera thinks it's facing down the x-axis, while tf thinks it's
facing down the z, so we put in (z, -x, y)
y also has to be negated here, not sure why??? """
points[i] = Point(transformed[2,0], -transformed[0,0], -transformed[1,0])
# Old method
# points[i].x = ( 1 - (2*points[i].x/self.xWid) * self.aspectXtoZ )
# points[i].y = ( 1 - (2*points[i].y/self.yHei) * self.aspectYtoZ )
""" This should add the points for each plane in the correct order,
counter-clockwise if looking from within the pyramid """
origin = Point(0,0,0)
for point1,point2 in zip(points, points[1:]+[points[0]]):
poly.planes.append( self.GenPlane(origin, point1, point2) )
# finally, if front is positive, then we want to produce a bounding
# plane there, facing away from the camera. So, if we stand at the
# origin, it should be a clockwise pattern.
if front > 0:
point1 = Point(front, 0,0)
point2 = Point(front, 1,0)
point3 = Point(front, 1,1)
poly.planes.append( self.GenPlane(point1, point2, point3) )
# we are at last ready to speak to the world!
self.bb_publisher.publish(poly)
""" This bit is for the purpose of testing the intersector
It publishes a cube that should intersect with the
bounding pyramid generated for the blue_block
"""
poly.sensor = "testing"
poly.algorithm = "testing"
poly.planes = []
# Front and Back planes
poly.planes.append( self.GenPlaneFromWorldPoints(Point(2,0,0),Point(2,1,0),Point(2,0,1)) )
poly.planes.append( self.GenPlaneFromWorldPoints(Point(1,0,0),Point(1,0,1),Point(1,1,0)) )
# Right and Left planes
poly.planes.append( self.GenPlaneFromWorldPoints(Point(0,-2,0),Point(1,-2,0),Point(0,-2,1)) )
poly.planes.append( self.GenPlaneFromWorldPoints(Point(0,-1,0),Point(0,-1,1),Point(1,-1,0)) )
# Top and Bottom planes
poly.planes.append( self.GenPlaneFromWorldPoints(Point(0,0,1),Point(1,0,1),Point(0,1,1)) )
poly.planes.append( self.GenPlaneFromWorldPoints(Point(0,0,0),Point(0,1,0),Point(1,0,0)) )
self.bb_publisher.publish(poly)
def init():
output_name = "intersector_input"
publisher = rospy.Publisher(output_name, Convex_Space)
rospy.init_node('intersector')
print "Sleep for 2 seconds to allow the 3d_intersector time to initialize..."
rospy.Rate(.5).sleep()
print "Sleep done..."
print "Sending recorded shape information..."
# the following was recorded from "rostopic echo /intersector_input" during a run of one of our Gazebo tests
# ---
# planes: [
# a: 0.0
# b: 0.0
# c: 1.0
# d: -1.0,
# a: 0.0
# b: 0.0
# c: -1.0
# d: 0.0]
# sensor: horizontal_slicer
# algorithm: horizontal_slicer
# aux_payload: slicer:horizontal
# ---
# planes: [
# a: -0.0507812052965
# b: 0.0
# c: -0.0324326232076
# d: -0.0972978696227,
# a: 3.45435147153e-08
# b: 0.048828125
# c: -0.0260353088379
# d: -0.0781059265137,
# a: 0.0507812090218
# b: 0.0
# c: 0.0299530699849
# d: 0.0898592099547,
# a: -3.12536556635e-08
# b: -0.048828125
# c: 0.0235557556152
# d: 0.0706672668457]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:blue
# ---
# planes: [
# a: -5.19797671572e-09
# b: -0.025390625
# c: 0.0039176940918
# d: 0.0117530822754,
# a: -0.0253906045109
# b: 0.0
# c: -0.0159187652171
# d: -0.0477562956512,
# a: 6.05333960735e-09
# b: 0.025390625
# c: -0.00456237792969
# d: -0.0136871337891,
# a: 0.0253906045109
# b: 0.0
# c: 0.0152740813792
# d: 0.0458222441375]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:white
# ---
# planes: [
# a: 6.38232577899e-09
# b: -0.025390625
# c: -0.00481033325195
# d: -0.0144309997559,
# a: -0.0253906026483
# b: 0.0
# c: -0.0162163116038
# d: -0.0486489348114,
# a: -5.52696244327e-09
# b: 0.025390625
# c: 0.00416564941406
# d: 0.0124969482422,
# a: 0.0253906045109
# b: 0.0
# c: 0.0155716277659
# d: 0.0467148832977]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:green
# ---
# planes: [
# a: 3.46700481657e-08
# b: -0.048828125
# c: -0.0261306762695
# d: -0.0783920288086,
# a: -0.0507812052965
# b: 0.0
# c: -0.0324326232076
# d: -0.0972978696227,
# a: -3.13801891139e-08
# b: 0.048828125
# c: 0.0236511230469
# d: 0.0709533691406,
# a: 0.0507812090218
# b: 0.0
# c: 0.0299530699849
# d: 0.0898592099547]
# sensor: /overhead_cam
# algorithm: ian3
# aux_payload: ian3:red
# ---
space = Convex_Space()
space.sensor = "horizontal_slicer"
space.algorithm = "horizontal_slicer"
space.aux_payload = "slicer:horizontal"
space.planes = []
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = 1
plane.d = -1
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = -1
plane.d = 0
space.planes.append(plane)
# print "PUBLISHING: %s" % space
publisher.publish(space)
# ----
space = Convex_Space()
space.sensor = "/overhead_cam"
space.algorithm = "ian3"
space.aux_payload = "ian3:blue"
space.planes = []
plane = Plane()
plane.a = -0.0507812052965
plane.b = 0.0
plane.c = -0.0324326232076
plane.d = -0.0972978696227
space.planes.append(plane)
plane = Plane()
plane.a = 3.45435147153e-08
plane.b = 0.048828125
plane.c = -0.0260353088379
plane.d = -0.0781059265137
space.planes.append(plane)
plane = Plane()
plane.a = 0.0507812090218
plane.b = 0.0
plane.c = 0.0299530699849
plane.d = 0.0898592099547
space.planes.append(plane)
plane = Plane()
plane.a = -3.12536556635e-08
plane.b = -0.048828125
plane.c = 0.0235557556152
plane.d = 0.0706672668457
space.planes.append(plane)
# print "PUBLISHING: %s" % space
publisher.publish(space)
# ----
space = Convex_Space()
space.sensor = "/overhead_cam"
space.algorithm = "ian3"
space.aux_payload = "ian3:white"
space.planes = []
plane = Plane()
plane.a = -5.19797671572e-09
plane.b = -0.025390625
plane.c = 0.0039176940918
plane.d = 0.0117530822754
space.planes.append(plane)
plane = Plane()
plane.a = -0.0253906045109
plane.b = 0.0
plane.c = -0.0159187652171
plane.d = -0.0477562956512
space.planes.append(plane)
plane = Plane()
plane.a = 6.05333960735e-09
plane.b = 0.025390625
plane.c = -0.00456237792969
plane.d = -0.0136871337891
space.planes.append(plane)
plane = Plane()
plane.a = 0.0253906045109
plane.b = 0.0
plane.c = 0.0152740813792
plane.d = 0.0458222441375
space.planes.append(plane)
# print "PUBLISHING: %s" % space
publisher.publish(space)
# ----
space = Convex_Space()
space.sensor = "/overhead_cam"
space.algorithm = "ian3"
space.aux_payload = "ian3:green"
space.planes = []
plane = Plane()
plane.a = 6.38232577899e-09
plane.b = -0.025390625
plane.c = -0.00481033325195
plane.d = -0.0144309997559
space.planes.append(plane)
plane = Plane()
plane.a = -0.0253906026483
plane.b = 0.0
plane.c = -0.0162163116038
plane.d = -0.0486489348114
space.planes.append(plane)
plane = Plane()
plane.a = -5.52696244327e-09
plane.b = 0.025390625
plane.c = 0.00416564941406
plane.d = 0.0124969482422
space.planes.append(plane)
plane = Plane()
plane.a = 0.0253906045109
plane.b = 0.0
plane.c = 0.0155716277659
plane.d = 0.0467148832977
space.planes.append(plane)
# print "PUBLISHING: %s" % space
publisher.publish(space)
# ----
space = Convex_Space()
space.sensor = "/overhead_cam"
space.algorithm = "ian3"
space.aux_payload = "ian3:red"
space.planes = []
plane = Plane()
plane.a = 3.46700481657e-08
plane.b = -0.048828125
plane.c = -0.0261306762695
plane.d = -0.0783920288086
space.planes.append(plane)
plane = Plane()
plane.a = -0.0507812052965
plane.b = 0.0
plane.c = -0.0324326232076
plane.d = -0.0972978696227
space.planes.append(plane)
plane = Plane()
plane.a = -3.13801891139e-08
plane.b = 0.048828125
plane.c = 0.0236511230469
plane.d = 0.0709533691406
space.planes.append(plane)
plane = Plane()
plane.a = 0.0507812090218
plane.b = 0.0
plane.c = 0.0299530699849
plane.d = 0.0898592099547
space.planes.append(plane)
# print "PUBLISHING: %s" % space
publisher.publish(space)
# END OF RECORDED PLANES
return
# this is a horizontal slicer
space = Convex_Space()
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = 1
plane.d = -1
space.planes.append(plane);
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = -1
plane.d = 0
space.planes.append(plane);
space.sensor = "horizontal_slicer"
space.algorithm = "horizontal_slicer"
space.aux_payload = "slicer:horizontal"
publisher.publish(space)
print "Just published a horizontal_slicer"
rospy.Rate(1).sleep()
# this generates something approximating the sort of pyramid which
# camera2boundingBox would produce.
#
# Characteristic points:
# Camera:
# (0, 0, 3)
# Planes:
# (3, 3, 0)
# (3, 6, 0)
# (6, 3, 0)
# (6, 6, 0)
space = Convex_Space()
plane = Plane()
plane.a = -1
plane.b = 0
plane.c = -1
plane.d = 3
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = -1
plane.c = -1
plane.d = 3
space.planes.append(plane)
plane = Plane()
plane.a = 0.5
plane.b = 0
plane.c = 1
plane.d = -3
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = 0.5
plane.c = 1
plane.d = -3
space.planes.append(plane)
space.sensor = "pseudo_pyramid"
space.algorithm = "pseudo_pyramid"
space.aux_payload = ""
publisher.publish(space)
print "Just published a pseudo-pyramid"
rospy.Rate(1).sleep()
# test a few passes where we gradually reduce the box size.
for radius in inclusive_range(10*1000, 5*1000, -1000):
space = Convex_Space()
plane = Plane()
plane.a = 1
plane.b = 0
plane.c = 0
plane.d = -radius
print "plane: %s" % plane
space.planes.append(plane)
plane = Plane()
plane.a = -1
plane.b = 0
plane.c = 0
plane.d = -radius
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = 1
plane.c = 0
plane.d = -radius
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = -1
plane.c = 0
plane.d = -radius
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = 1
plane.d = -radius
space.planes.append(plane)
plane = Plane()
plane.a = 0
plane.b = 0
plane.c = -1
plane.d = -radius
space.planes.append(plane)
space.sensor = "plane_slicer"
space.algorithm = "plane_slicer"
space.aux_payload = "plane_slicer_radius=%d" % radius
publisher.publish(space)
print "Just published a plane-slicing space with radius=%d" % radius
rospy.Rate(1).sleep()
# remember that the initial box, defined in 3d_intersector, is +- 10*1000 in each dimension.
#
# With radius == 10*1000, we get 3 key points, which are the exact centers of 3 different
# faces; the plane slices through those. However, it works out that the plane also slices
# through the 3 nearby corners of the original box.
#
# With radius == 20*1000, we get 3 key points, but these are now midpoints of edges, instead
# of the center of faces. Thus, each plane just slices off one corner of the cube.
#
# We'll test with 20k, then 10k, and see how our intersector handles it.
for radius in inclusive_range(20*1000, 10*1000, -10*1000):
space = Convex_Space()
for a in range(-1, 1+1, 2):
for b in range(-1, 1+1, 2):
for c in range(-1, 1+1, 2):
plane = Plane()
plane.a = a
plane.b = b
plane.c = c
plane.d = -radius # need negative d value, since we are adding it to the left-hand side of the inequality, and then checking if the value is small enough.
space.planes.append(plane)
space.sensor = "corner_slicer"
space.algorithm = "corner_slicer:radius=%d" % radius
space.aux_payload = "corner_slicer_radius=%d" % radius
publisher.publish(space)
print "Just published a corner-slicing space with radius=%d" % radius
rospy.Rate(1).sleep()
