def checkRange(self, src, lowBound, highBound):
size = im.size(src)
mask = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
gt_low = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
cv.CmpS(src, lowBound, gt_low, cv.CV_CMP_GT)
lt_high = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
cv.CmpS(src, highBound, lt_high, cv.CV_CMP_LT)
cv.And(gt_low, lt_high, mask)
return mask
def split(image):
ch1 = cv.CreateImage(cv.GetSize(image), image.depth, image.nChannels)
ch2 = cv.CreateImage(cv.GetSize(image), image.depth, image.nChannels)
ch3 = cv.CreateImage(cv.GetSize(image), image.depth, image.nChannels)
ch4 = cv.CreateImage(cv.GetSize(image), image.depth, image.nChannels)
cv.CmpS(image, mygray, ch1, cv.CV_CMP_EQ)
cv.CmpS(image, myblue, ch2, cv.CV_CMP_EQ)
cv.CmpS(image, mylgrn, ch3, cv.CV_CMP_EQ)
cv.CmpS(image, mydgrn, ch4, cv.CV_CMP_EQ)
return (ch1, ch2, ch3, ch4)
def equivalent_region_nearest(nearest_building):
''' find the equivalent class region via nearest building
'''
equ_class_region = cv.CreateImage(
(self.map_img.width, self.map_img.height), cv.IPL_DEPTH_8U, 1)
bid = nearest_building.bid
cv.CmpS(self.buildingMgr.empty_img, bid, equ_class_region,
cv.CV_CMP_EQ)
return equ_class_region
def precornerdetect(image):
# assume that the image is floating-point
corners = cv.CloneMat(image)
cv.PreCornerDetect(image, corners, 3)
dilated_corners = cv.CloneMat(image)
cv.Dilate(corners, dilated_corners, None, 1)
corner_mask = cv.CreateMat(image.rows, image.cols, cv.CV_8UC1)
cv.Sub(corners, dilated_corners, corners)
cv.CmpS(corners, 0, corner_mask, cv.CV_CMP_GE)
return (corners, corner_mask)
def handle_stereo(self, qq):
(lmsg, lcmsg, rmsg, rcmsg) = qq
limg = CvBridge().imgmsg_to_cv(lmsg, "mono8")
rimg = CvBridge().imgmsg_to_cv(rmsg, "mono8")
if 0:
cv.SaveImage("/tmp/l%06d.png" % self.i, limg)
cv.SaveImage("/tmp/r%06d.png" % self.i, rimg)
self.i += 1
scm = image_geometry.StereoCameraModel()
scm.fromCameraInfo(lcmsg, rcmsg)
bm = cv.CreateStereoBMState()
if "wide" in rospy.resolve_name("stereo"):
bm.numberOfDisparities = 160
if 0:
disparity = cv.CreateMat(limg.rows, limg.cols, cv.CV_16SC1)
started = time.time()
cv.FindStereoCorrespondenceBM(limg, rimg, disparity, bm)
print time.time() - started
ok = cv.CreateMat(limg.rows, limg.cols, cv.CV_8UC1)
cv.CmpS(disparity, 0, ok, cv.CV_CMP_GT)
cv.ShowImage("limg", limg)
cv.ShowImage("disp", ok)
cv.WaitKey(6)
self.track(CvBridge().imgmsg_to_cv(lmsg, "rgb8"))
if len(self.tracking) == 0:
print "No markers found"
for code, corners in self.tracking.items():
corners3d = []
for (x, y) in corners:
limr = cv.GetSubRect(limg, (0, y - bm.SADWindowSize / 2, limg.cols, bm.SADWindowSize + 1))
rimr = cv.GetSubRect(rimg, (0, y - bm.SADWindowSize / 2, rimg.cols, bm.SADWindowSize + 1))
tiny_disparity = cv.CreateMat(limr.rows, limg.cols, cv.CV_16SC1)
cv.FindStereoCorrespondenceBM(limr, rimr, tiny_disparity, bm)
if tiny_disparity[7, x] < 0:
return
corners3d.append(scm.projectPixelTo3d((x, y), tiny_disparity[7, x] / 16.))
if 0:
cv.ShowImage("d", disparity)
(a, b, c, d) = [numpy.array(pt) for pt in corners3d]
def normal(s, t):
return (t - s) / numpy.linalg.norm(t - s)
x = PyKDL.Vector(*normal(a, b))
y = PyKDL.Vector(*normal(a, d))
f = PyKDL.Frame(PyKDL.Rotation(x, y, x * y), PyKDL.Vector(*a))
msg = pm.toMsg(f)
# print "%10f %10f %10f" % (msg.position.x, msg.position.y, msg.position.z)
print code, msg.position.x, msg.position.y, msg.position.z
self.broadcast(lmsg.header, code, msg)
def dct_hash(img):
img = float_version(img)
small_img = cv.CreateImage((32, 32), 32, 1)
cv.Resize(img[20:190, 20:205], small_img)
dct = cv.CreateMat(32, 32, cv.CV_32FC1)
cv.DCT(small_img, dct, cv.CV_DXT_FORWARD)
dct = dct[1:9, 1:9]
avg = cv.Avg(dct)[0]
dct_bit = cv.CreateImage((8, 8), 8, 1)
cv.CmpS(dct, avg, dct_bit, cv.CV_CMP_GT)
return [dct_bit[y, x] == 255.0 for y in xrange(8) for x in xrange(8)]
def detectMotion(self, curr):
assert (curr.nChannels == 1)
if len(self.history_frames) < self.nHistory:
self.history_frames.append(curr)
return curr
else:
oldest_frame = self.history_frames.pop(0)
self.history_frames.append(curr)
size = (curr.width, curr.height)
motion_frame = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
cv.AbsDiff(oldest_frame, curr, motion_frame)
cv.CmpS(motion_frame, self.threshold, motion_frame, cv.CV_CMP_GT)
# Eliminate disperse pixels, which occur because of
# the noise of the camera
img_temp = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1)
cv.Erode(motion_frame, img_temp)
cv.Dilate(img_temp, motion_frame)
return motion_frame
def get_near_region_mask(self, blds):
regions = [bd.near_region_poly for bd in blds]
c = cg.rects_intersection(regions, self.label_img.width,
self.label_img.height)
# subtract buildings
equ_class_region = cv.CreateImage(
(self.label_img.width, self.label_img.height), cv.IPL_DEPTH_8U, 1)
canvas = cv.CloneImage(self.label_img)
cv.FillPoly(equ_class_region, [c], im.color.blue)
cv.CmpS(canvas, 0, canvas, cv.CV_CMP_EQ)
cv.And(equ_class_region, canvas, equ_class_region)
# subtract near of near's neighbor
near_of_near = set(self.buildings)
for bd in blds:
near_of_near = near_of_near.union(bd.near_set)
near_of_near.difference_update(set(blds))
near_regions = [bd.near_region_poly for bd in near_of_near]
for reg, bd in zip(near_regions, near_of_near):
if cg.has_intersection(equ_class_region, reg, self.label_img.width,
self.label_img.height):
cg.sub_intersection(equ_class_region, reg,
self.label_img.width,
self.label_img.height)
# equ_class_region -= near of near via nearest
for bd in near_of_near:
eq_region = self.get_equivalent_region_via_nearest(bd)
c = im.find_contour(eq_region)
while c:
if cg.has_intersection(equ_class_region, list(c),
self.label_img.width,
self.label_img.height):
cg.sub_intersection(equ_class_region, list(c),
self.label_img.width,
self.label_img.height)
c = c.h_next()
return equ_class_region
def init_buildings(buildingIDs, label_img):
bld_mask = cv.CreateImage((label_img.width, label_img.height),
cv.IPL_DEPTH_8U, 1)
buildings = []
for bid in buildingIDs:
cv.CmpS(label_img, bid, bld_mask, cv.CV_CMP_EQ)
buildings.append(Building(bid, im.find_contour(bld_mask)))
for rb in buildings:
for sb in buildings:
if sb.bid == rb.bid:
continue
if rb.is_near_me(sb):
rb.near_set.add(sb)
if rb.is_north_me(sb):
rb.north_set.add(sb)
if rb.is_south_me(sb):
rb.south_set.add(sb)
if rb.is_east_me(sb):
rb.east_set.add(sb)
if rb.is_west_me(sb):
rb.west_set.add(sb)
return buildings
def init_empty_img(self):
# calculate Union{ near_regions } for all building
nearby_region_polys = [bd.near_region_poly for bd in self.buildings]
all_near_regions = cv.CreateImage(
(self.label_img.width, self.label_img.height), cv.IPL_DEPTH_8U, 1)
cv.FillPoly(all_near_regions, [nearby_region_polys[0]], im.color.blue)
for poly in nearby_region_polys:
tmp_canvas = cv.CreateImage(
(self.label_img.width, self.label_img.height), cv.IPL_DEPTH_8U,
1)
cv.FillPoly(tmp_canvas, [poly], im.color.blue)
cv.Or(tmp_canvas, all_near_regions, all_near_regions)
# find the "empty" region
empty_region = cv.CreateImage(
(self.label_img.width, self.label_img.height), cv.IPL_DEPTH_8U, 1)
cv.CmpS(all_near_regions, 0, empty_region, cv.CV_CMP_EQ)
for ele in it.nonzero_indices(cv.GetMat(empty_region)):
y, x = ele
y, x = int(y), int(x)
nearest_bd = self.get_nearest_building(x, y)
cv.Set2D(empty_region, y, x, nearest_bd.bid)
return empty_region
ctime = time()
if ctime - stime >= 1:
print '%ifps' % (framecnt)
framecnt = 0
stime = ctime
# Get original image
img = cv.QueryFrame(cam)
# Adaptive diff
#cv.Smooth(img, col, cv.CV_GAUSSIAN, 3, 0)
cv.AddWeighted(img, 1 - ADAPT, avg, ADAPT, 0, avg)
cv.AbsDiff(img, avg, col)
cv.CvtColor(col, col, cv.CV_RGB2HSV)
cv.Split(col, None, None, val, None)
cv.CmpS(val, TH, val, cv.CV_CMP_GE)
#cv.Dilate(val, val, None, 18)
#cv.Erode(val, val, None, 10)
dif = cv.CountNonZero(val)
# Show image if it's radically new
if (dif < PIXCOUNT):
if act < 0:
act = ACTLOW
else:
act -= 1
else:
if act > 0:
act = ACTHIGH
else:
act += 1
# histogram creation
#temp = cv.CreateMat(height,width,cv.CV_32FC1) #cv.CV_8UC1)
#cv.SetZero(temp)
#cv.ConvertScale(themap,temp,40,0)
#temp2 = cv.CreateMat(height,width,cv.CV_32FC1)
#cv.SetZero(temp2)
#cv.Pow(temp,temp2,0.5)
#cv.SaveImage("histogram.png",temp2)
cv.Smooth(themap, themap, cv.CV_GAUSSIAN, gaussian_blur,gaussian_blur)
cv.SaveImage("map.png",themap)
(minval,maxval,minloc,maxloc)=cv.MinMaxLoc(themap)
print "Min: "+`minval`+" max: "+`maxval`
cv.ConvertScale(themap,mask,255.0/maxval,0);
cv.SaveImage("mask.png",mask)
cv.CmpS(themap,mask_threshold,mask,cv.CV_CMP_GT)
cv.SaveImage("thresholded.png",mask)
#contour = cv.FindContours(mask,cv.CreateMemStorage(),cv.CV_RETR_CCOMP,cv.CV_CHAIN_APPROX_SIMPLE)
chain = cv.FindContours(mask,cv.CreateMemStorage(),cv.CV_RETR_CCOMP,cv.CV_CHAIN_CODE)
contour = cv.ApproxChains(chain,cv.CreateMemStorage(),cv.CV_CHAIN_APPROX_NONE,0,100,1)
img = cv.CreateMat(height,width,cv.CV_8UC3)
cv.SetZero(img)
cv.DrawContours(img,contour,(255,255,255),(0,255,0),6,1)
cv.SaveImage("contours.png",img)
img = cv.CreateMat(height,width,cv.CV_8UC3)
cv.SetZero(img)
# # create the voronoi graph
def get_equivalent_region_via_nearest(self, bld):
equ_class_region = cv.CreateImage(
(self.label_img.width, self.label_img.height), cv.IPL_DEPTH_8U, 1)
bid = bld.bid
cv.CmpS(self.empty_img, bid, equ_class_region, cv.CV_CMP_EQ)
return equ_class_region
import pyvision as pv
import PIL, cv
ilog = pv.ImageLog()
im = pv.Image("baboon.jpg")
# In PIL
pil = im.asPIL()
gray = pil.convert('L')
thresh = PIL.Image.eval(gray, lambda x: 255 * (x > 127.5))
ilog(pv.Image(thresh), "PILThresh")
#in Scipy
mat = im.asMatrix2D()
thresh = mat > 127.5
ilog(pv.Image(1.0 * thresh), "ScipyThresh")
#in OpenCV
cvim = im.asOpenCVBW()
dest = cv.CreateImage(im.size, cv.IPL_DEPTH_8U, 1)
cv.CmpS(cvim, 127.5, dest, cv.CV_CMP_GT)
ilog(pv.Image(dest), "OpenCVThresh")
ilog.show()
def get_point_cloud(self):
# Scan the scene
col_associations = self.get_projector_line_associations()
# Project white light onto the scene to get the intensities of each pixel (for coloring our point cloud)
illumination_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.Set(illumination_projection, 255)
intensities_image = self.get_picture_of_projection(
illumination_projection)
# Turn projector off when done with it
off_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width, cv.CV_8UC1)
cv.SetZero(off_projection)
off_image_message = self.bridge.cv_to_imgmsg(off_projection,
encoding="mono8")
self.set_projection(off_image_message)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(self.camera_info)
valid_points_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(col_associations, -1, valid_points_mask, cv.CV_CMP_NE)
number_of_valid_points = cv.CountNonZero(valid_points_mask)
rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC2)
scanline_associations = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC1)
intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
i = 0
for row in range(self.camera_info.height):
for col in range(self.camera_info.width):
if valid_points_mask[row, col] != 0:
rectified_camera_coordinates[0, i] = (col, row)
scanline_associations[0, i] = col_associations[row, col]
intensities[0, i] = intensities_image[row, col]
i += 1
cv.UndistortPoints(rectified_camera_coordinates,
rectified_camera_coordinates,
camera_model.intrinsicMatrix(),
camera_model.distortionCoeffs())
# Convert scanline numbers to plane normal vectors
planes = self.scanline_numbers_to_planes(scanline_associations)
camera_rays = project_pixels_to_3d_rays(rectified_camera_coordinates,
camera_model)
intersection_points = ray_plane_intersections(camera_rays, planes)
self.point_cloud_msg = sensor_msgs.msg.PointCloud()
self.point_cloud_msg.header.stamp = rospy.Time.now()
self.point_cloud_msg.header.frame_id = 'base_link'
channels = []
channel = sensor_msgs.msg.ChannelFloat32()
channel.name = "intensity"
points = []
intensities_list = []
for i in range(number_of_valid_points):
point = geometry_msgs.msg.Point32()
point.x = intersection_points[0, i][0]
point.y = intersection_points[0, i][1]
point.z = intersection_points[0, i][2]
if point.z < 0:
print scanline_associations[0, i]
print rectified_camera_coordinates[0, i]
points.append(point)
intensity = intensities[0, i]
intensities_list.append(intensity)
channel.values = intensities_list
channels.append(channel)
self.point_cloud_msg.channels = channels
self.point_cloud_msg.points = points
return self.point_cloud_msg
def get_projector_line_associations(self):
rospy.loginfo("Scanning...")
positives = []
negatives = []
for i in range(self.number_of_projection_patterns):
positives.append(
self.get_picture_of_projection(
self.predistorted_positive_projections[i]))
negatives.append(
self.get_picture_of_projection(
self.predistorted_negative_projections[i]))
rospy.loginfo("Thresholding...")
strike_sum = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.SetZero(strike_sum)
gray_codes = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.SetZero(gray_codes)
for i in range(self.number_of_projection_patterns):
difference = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.Sub(positives[i], negatives[i], difference)
absolute_difference = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_8UC1)
cv.AbsDiff(positives[i], negatives[i], absolute_difference)
#Mark all the pixels that were "too close to call" and add them to the running total
strike_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(absolute_difference, self.threshold, strike_mask,
cv.CV_CMP_LT)
strikes = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.Set(strikes, 1, strike_mask)
cv.Add(strikes, strike_sum, strike_sum)
#Set the corresponding bit in the gray_code
bit_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(difference, 0, bit_mask, cv.CV_CMP_GT)
bit_values = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_32SC1)
cv.Set(bit_values, 2**i, bit_mask)
cv.Or(bit_values, gray_codes, gray_codes)
rospy.loginfo("Decoding...")
# Decode every gray code into binary
projector_line_associations = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_32SC1)
cv.Copy(gray_codes, projector_line_associations)
for i in range(
cv.CV_MAT_DEPTH(cv.GetElemType(projector_line_associations)),
-1, -1):
projector_line_associations_bitshifted_right = cv.CreateMat(
self.camera_info.height, self.camera_info.width, cv.CV_32SC1)
#Using ConvertScale with a shift of -0.5 to do integer division for bitshifting right
cv.ConvertScale(projector_line_associations,
projector_line_associations_bitshifted_right,
(2**-(2**i)), -0.5)
cv.Xor(projector_line_associations,
projector_line_associations_bitshifted_right,
projector_line_associations)
rospy.loginfo("Post processing...")
# Remove all pixels that were "too close to call" more than once
strikeout_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(strike_sum, 1, strikeout_mask, cv.CV_CMP_GT)
cv.Set(projector_line_associations, -1, strikeout_mask)
# Remove all pixels that don't decode to a valid scanline number
invalid_scanline_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_8UC1)
cv.InRangeS(projector_line_associations, 0, self.number_of_scanlines,
invalid_scanline_mask)
cv.Not(invalid_scanline_mask, invalid_scanline_mask)
cv.Set(projector_line_associations, -1, invalid_scanline_mask)
self.display_scanline_associations(projector_line_associations)
return projector_line_associations
def get_point_cloud(self):
# Scan the scene
pixel_associations = self.get_pixel_associations()
# Project white light onto the scene to get the intensities of each picture (for coloring our point cloud)
illumination_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.Set(illumination_projection, 255)
intensities_image = self.get_picture_of_projection(
illumination_projection)
# Turn projector off when done with it
off_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width, cv.CV_8UC1)
cv.SetZero(off_projection)
off_image_message = self.bridge.cv_to_imgmsg(off_projection,
encoding="mono8")
self.set_projection(off_image_message)
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(self.camera_info)
projector_model = PinholeCameraModel()
projector_model.fromCameraInfo(self.projector_info)
projector_to_camera_rotation_matrix = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(self.projector_to_camera_rotation_vector,
projector_to_camera_rotation_matrix)
pixel_associations_x = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_32SC1)
pixel_associations_y = cv.CreateMat(self.camera_info.height,
self.camera_info.width,
cv.CV_32SC1)
cv.Split(pixel_associations, pixel_associations_x,
pixel_associations_y, None, None)
valid_points_mask_x = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(pixel_associations_x, -1, valid_points_mask_x, cv.CV_CMP_NE)
valid_points_mask_y = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.CmpS(pixel_associations_y, -1, valid_points_mask_y, cv.CV_CMP_NE)
valid_points_mask = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.And(valid_points_mask_x, valid_points_mask_y, valid_points_mask)
number_of_valid_points = cv.CountNonZero(valid_points_mask)
rectified_camera_coordinates = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC2)
rectified_projector_coordinates = cv.CreateMat(1,
number_of_valid_points,
cv.CV_32FC2)
intensities = cv.CreateMat(1, number_of_valid_points, cv.CV_8UC1)
i = 0
for row in range(self.camera_info.height):
for col in range(self.camera_info.width):
if valid_points_mask[row, col] != 0:
rectified_camera_coordinates[0, i] = (col, row)
rectified_projector_coordinates[0, i] = pixel_associations[
row, col]
intensities[0, i] = intensities_image[row, col]
i += 1
cv.UndistortPoints(rectified_camera_coordinates,
rectified_camera_coordinates,
camera_model.intrinsicMatrix(),
camera_model.distortionCoeffs())
# Convert scanline numbers to pixel numbers
cv.AddS(rectified_projector_coordinates, 0.5,
rectified_projector_coordinates)
cv.ConvertScale(rectified_projector_coordinates,
rectified_projector_coordinates,
self.pixels_per_scanline)
# Rectify the projector pixels
cv.UndistortPoints(rectified_projector_coordinates,
rectified_projector_coordinates,
projector_model.intrinsicMatrix(),
projector_model.distortionCoeffs())
camera_rays = projectPixelsTo3dRays(rectified_camera_coordinates,
camera_model)
projector_rays = projectPixelsTo3dRays(rectified_projector_coordinates,
projector_model)
# Bring the projector rays into camera coordinates
cv.Transform(projector_rays, projector_rays,
projector_to_camera_rotation_matrix)
camera_centers = cv.CreateMat(1, number_of_valid_points, cv.CV_32FC3)
cv.SetZero(camera_centers)
projector_centers = cv.CreateMat(1, number_of_valid_points,
cv.CV_32FC3)
cv.Set(projector_centers, self.projector_to_camera_translation_vector)
intersection_points = line_line_intersections(camera_centers,
camera_rays,
projector_centers,
projector_rays)
self.point_cloud_msg = sensor_msgs.msg.PointCloud()
self.point_cloud_msg.header.stamp = rospy.Time.now()
self.point_cloud_msg.header.frame_id = 'base_link'
channels = []
channel = sensor_msgs.msg.ChannelFloat32()
channel.name = "intensity"
points = []
intensities_list = []
for i in range(number_of_valid_points):
point = geometry_msgs.msg.Point32()
point.x = intersection_points[0, i][0]
point.y = intersection_points[0, i][1]
point.z = intersection_points[0, i][2]
points.append(point)
intensity = intensities[0, i]
intensities_list.append(intensity)
channel.values = intensities_list
channels.append(channel)
self.point_cloud_msg.channels = channels
self.point_cloud_msg.points = points
return self.point_cloud_msg
def detect(self):
self.detected = 0
cv.Smooth(self.grey, self.dst2, cv.CV_GAUSSIAN, 3)
cv.Laplace(self.dst2, self.d)
cv.CmpS(self.d, 8, self.d2, cv.CV_CMP_GT)
if self.onlyBlackCubes:
# can also detect on black lines for improved robustness
cv.CmpS(grey, 100, b, cv.CV_CMP_LT)
cv.And(b, d2, d2)
# these weights should be adaptive. We should always detect 100 lines
if self.lastdetected > self.dects:
self.THR = self.THR + 1
if self.lastdetected < self.dects:
self.THR = max(2, self.THR - 1)
self.li = cv.HoughLines2(self.d2, cv.CreateMemStorage(),
cv.CV_HOUGH_PROBABILISTIC, 1, 3.1415926 / 45,
self.THR, 10, 5)
# store angles for later
angs = []
for (p1, p2) in self.li:
# cv.Line(sg,p1,p2,(0,255,0))
a = atan2(p2[1] - p1[1], p2[0] - p1[0])
if a < 0:
a += pi
angs.append(a)
# log.info("THR %d, lastdetected %d, dects %d, houghlines %d, angles: %s" % (self.THR, self.lastdetected, self.dects, len(self.li), pformat(angs)))
# lets look for lines that share a common end point
t = 10
totry = []
for i in range(len(self.li)):
p1, p2 = self.li[i]
for j in range(i + 1, len(self.li)):
q1, q2 = self.li[j]
# test lengths are approximately consistent
dd1 = sqrt((p2[0] - p1[0]) * (p2[0] - p1[0]) +
(p2[1] - p1[1]) * (p2[1] - p1[1]))
dd2 = sqrt((q2[0] - q1[0]) * (q2[0] - q1[0]) +
(q2[1] - q1[1]) * (q2[1] - q1[1]))
if max(dd1, dd2) / min(dd1, dd2) > 1.3:
continue
matched = 0
if areclose(p1, q2, t):
IT = (avg(p1, q2), p2, q1, dd1)
matched = matched + 1
if areclose(p2, q2, t):
IT = (avg(p2, q2), p1, q1, dd1)
matched = matched + 1
if areclose(p1, q1, t):
IT = (avg(p1, q1), p2, q2, dd1)
matched = matched + 1
if areclose(p2, q1, t):
IT = (avg(p2, q1), q2, p1, dd1)
matched = matched + 1
if matched == 0:
# not touching at corner... try also inner grid segments hypothesis?
self.p1 = (float(p1[0]), float(p1[1]))
self.p2 = (float(p2[0]), float(p2[1]))
self.q1 = (float(q1[0]), float(q1[1]))
self.q2 = (float(q2[0]), float(q2[1]))
success, (ua, ub), (x, y) = intersect_seg(
self.p1[0], self.p2[0], self.q1[0], self.q2[0],
self.p1[1], self.p2[1], self.q1[1], self.q2[1])
if success and ua > 0 and ua < 1 and ub > 0 and ub < 1:
# if they intersect
# cv.Line(sg, p1, p2, (255,255,255))
ok1 = 0
ok2 = 0
if abs(ua - 1.0 / 3) < 0.05:
ok1 = 1
if abs(ua - 2.0 / 3) < 0.05:
ok1 = 2
if abs(ub - 1.0 / 3) < 0.05:
ok2 = 1
if abs(ub - 2.0 / 3) < 0.05:
ok2 = 2
if ok1 > 0 and ok2 > 0:
# ok these are inner lines of grid
# flip if necessary
if ok1 == 2:
self.p1, self.p2 = self.p2, self.p1
if ok2 == 2:
self.q1, self.q2 = self.q2, self.q1
# both lines now go from p1->p2, q1->q2 and
# intersect at 1/3
# calculate IT
z1 = (self.q1[0] + 2.0 / 3 *
(self.p2[0] - self.p1[0]), self.q1[1] +
2.0 / 3 * (self.p2[1] - self.p1[1]))
z2 = (self.p1[0] + 2.0 / 3 *
(self.q2[0] - self.q1[0]), self.p1[1] +
2.0 / 3 * (self.q2[1] - self.q1[1]))
z = (self.p1[0] - 1.0 / 3 *
(self.q2[0] - self.q1[0]), self.p1[1] -
1.0 / 3 * (self.q2[1] - self.q1[1]))
IT = (z, z1, z2, dd1)
matched = 1
# only single one matched!! Could be corner
if matched == 1:
# also test angle
a1 = atan2(p2[1] - p1[1], p2[0] - p1[0])
a2 = atan2(q2[1] - q1[1], q2[0] - q1[0])
if a1 < 0:
a1 += pi
if a2 < 0:
a2 += pi
ang = abs(abs(a2 - a1) - pi / 2)
if ang < 0.5:
totry.append(IT)
# cv.Circle(sg, IT[0], 5, (255,255,255))
# now check if any points in totry are consistent!
# t=4
res = []
for i in range(len(totry)):
p, p1, p2, dd = totry[i]
a1 = atan2(p1[1] - p[1], p1[0] - p[0])
a2 = atan2(p2[1] - p[1], p2[0] - p[0])
if a1 < 0:
a1 += pi
if a2 < 0:
a2 += pi
dd = 1.7 * dd
evidence = 0
# cv.Line(sg,p,p2,(0,255,0))
# cv.Line(sg,p,p1,(0,255,0))
# affine transform to local coords
A = matrix([[p2[0] - p[0], p1[0] - p[0], p[0]],
[p2[1] - p[1], p1[1] - p[1], p[1]], [0, 0, 1]])
Ainv = A.I
v = matrix([[p1[0]], [p1[1]], [1]])
# check likelihood of this coordinate system. iterate all lines
# and see how many align with grid
for j in range(len(self.li)):
# test angle consistency with either one of the two angles
a = angs[j]
ang1 = abs(abs(a - a1) - pi / 2)
ang2 = abs(abs(a - a2) - pi / 2)
if ang1 > 0.1 and ang2 > 0.1:
continue
# test position consistency.
q1, q2 = self.li[j]
qwe = 0.06
# test one endpoint
v = matrix([[q1[0]], [q1[1]], [1]])
vp = Ainv * v
# project it
if vp[0, 0] > 1.1 or vp[0, 0] < -0.1:
continue
if vp[1, 0] > 1.1 or vp[1, 0] < -0.1:
continue
if abs(vp[0, 0] - 1 / 3.0) > qwe and abs(vp[0, 0] - 2 / 3.0) > qwe and \
abs(vp[1, 0] - 1 / 3.0) > qwe and abs(vp[1, 0] - 2 / 3.0) > qwe:
continue
# the other end point
v = matrix([[q2[0]], [q2[1]], [1]])
vp = Ainv * v
if vp[0, 0] > 1.1 or vp[0, 0] < -0.1:
continue
if vp[1, 0] > 1.1 or vp[1, 0] < -0.1:
continue
if abs(vp[0, 0] - 1 / 3.0) > qwe and abs(vp[0, 0] - 2 / 3.0) > qwe and \
abs(vp[1, 0] - 1 / 3.0) > qwe and abs(vp[1, 0] - 2 / 3.0) > qwe:
continue
# cv.Circle(sg, q1, 3, (255,255,0))
# cv.Circle(sg, q2, 3, (255,255,0))
# cv.Line(sg,q1,q2,(0,255,255))
evidence += 1
res.append((evidence, (p, p1, p2)))
minch = 10000
res.sort(reverse=True)
# log.info("dects %s, res:\n%s" % (self.dects, pformat(res)))
if len(res) > 0:
minps = []
pt = []
# among good observations find best one that fits with last one
for i in range(len(res)):
if res[i][0] > 0.05 * self.dects:
# OK WE HAVE GRID
p, p1, p2 = res[i][1]
p3 = (p2[0] + p1[0] - p[0], p2[1] + p1[1] - p[1])
# cv.Line(sg,p,p1,(0,255,0),2)
# cv.Line(sg,p,p2,(0,255,0),2)
# cv.Line(sg,p2,p3,(0,255,0),2)
# cv.Line(sg,p3,p1,(0,255,0),2)
# cen=(0.5*p2[0]+0.5*p1[0],0.5*p2[1]+0.5*p1[1])
# cv.Circle(sg, cen, 20, (0,0,255),5)
# cv.Line(sg, (0,cen[1]), (320,cen[1]),(0,255,0),2)
# cv.Line(sg, (cen[0],0), (cen[0],240), (0,255,0),2)
w = [p, p1, p2, p3]
p3 = (self.prevface[2][0] + self.prevface[1][0] -
self.prevface[0][0], self.prevface[2][1] +
self.prevface[1][1] - self.prevface[0][1])
tc = (self.prevface[0], self.prevface[1], self.prevface[2],
p3)
ch = compfaces(w, tc)
# log.info("ch %s, minch %s" % (ch, minch))
if ch < minch:
minch = ch
minps = (p, p1, p2)
# log.info("minch %d, minps:\n%s" % (minch, pformat(minps)))
if len(minps) > 0:
self.prevface = minps
if minch < 10:
# good enough!
self.succ += 1
self.pt = self.prevface
self.detected = 1
# log.info("detected %d, succ %d" % (self.detected, self.succ))
else:
self.succ = 0
# log.info("succ %d\n\n" % self.succ)
# we matched a few times same grid
# coincidence? I think NOT!!! Init LK tracker
if self.succ > 2:
# initialize features for LK
pt = []
for i in [1.0 / 3, 2.0 / 3]:
for j in [1.0 / 3, 2.0 / 3]:
pt.append(
(self.p0[0] + i * self.v1[0] + j * self.v2[0],
self.p0[1] + i * self.v1[1] + j * self.v2[1]))
self.features = pt
self.tracking = True
self.succ = 0
log.info("non-tracking -> tracking: succ %d" % self.succ)
def findColor(image, color):
result = cv.CreateImage(cv.GetSize(image), image.depth, 1)
cv.CmpS(image, color, result, cv.CV_CMP_EQ)
return result
def extractCol(image, col):
result = cv.CreateImage(cv.GetSize(image), cv.IPL_DEPTH_8U, 1)
cv.CmpS(image, col, result, cv.CV_CMP_EQ)
return result
