def check_undistortion(cam_opts):
cam = _build_test_camera(**cam_opts)
step = 5
border = 65
distorteds = []
for row in range(border, cam.height - border, step):
for col in range(border, cam.width - border, step):
distorted = [col, row]
distorteds.append(distorted)
npdistorted = np.array(distorteds, dtype=np.float)
src = numpy2opencv_pointmat(npdistorted)
dst = cv.CloneMat(src)
cv.UndistortPoints(src,
dst,
numpy2opencv_image(cam.get_K()),
numpy2opencv_image(cam.get_D()),
R=numpy2opencv_image(cam.get_rect()),
P=numpy2opencv_image(cam.get_P()))
undistorted_cv = opencv_pointmat2numpy(dst)
undistorted_np = cam.undistort(npdistorted)
assert undistorted_cv.shape == undistorted_np.shape
if cam.is_opencv_compatible():
assert np.allclose(undistorted_cv, undistorted_np)
else:
from nose.plugins.skip import SkipTest
raise SkipTest(
"Test %s is skipped: %s" %
(check_undistortion.__name__,
'camera model is not OpenCV compatible, skipping test'))
def __init__(self, config, rig):
self.config = config.analysis
self.rig = rig
x1 = np.linspace(0, rig.size[1],
config.analysis.outline_ptsperside).tolist()
y1 = np.linspace(0, rig.size[0],
config.analysis.outline_ptsperside).tolist()
points = []
for x in x1:
points.append([[x, 0.]])
for y in y1:
points.append([[rig.size[1], y]])
x1.reverse()
y1.reverse()
for x in x1:
points.append([[x, rig.size[0]]])
for y in y1:
points.append([[0., y]])
self.points_raw = np.asarray(points)
points_rect = cv.fromarray(self.points_raw.copy())
if self.rig.config.match_left:
cv.UndistortPoints(cv.fromarray(self.points_raw), points_rect,
rig.KK_left, rig.kc_left, rig.R1, rig.P1)
self.points_rect = (np.asarray(points_rect) -
[rig.P1[0, 2], rig.P1[1, 2]])[:, 0, :]
self.points_raw = (self.points_raw[:, 0, :] -
[rig.KK_left[0, 2], rig.KK_left[1, 2]])
else:
cv.UndistortPoints(cv.fromarray(self.points_raw), points_rect,
rig.KK_right, rig.kc_right, rig.R2, rig.P2)
self.points_rect = (np.asarray(points_rect) -
[rig.P2[0, 2], rig.P2[1, 2]])[:, 0, :]
self.points_raw = (self.points_raw[:, 0, :] -
[rig.KK_right[0, 2], rig.KK_right[1, 2]])
self.shape_rect = shape.Polygon(self.points_rect)
self.shape_raw = shape.Polygon(self.points_raw)
self.shape_im = self.shape_rect.intersection(self.shape_raw)
def undistort_points(self, src):
"""
:param src: N source pixel points (u,v) as an Nx2 matrix
:type src: :class:`cvMat`
Apply the post-calibration undistortion to the source points
"""
dst = cv.CloneMat(src)
cv.UndistortPoints(src, dst, self.intrinsics, self.distortion, R = self.R, P = self.P)
return dst
def pixel_position_matt(xpos, ypos, height, pitch, roll, yaw, C):
'''
find the offset on the ground in meters of a pixel in a ground image
given height above the ground in meters, and pitch/roll/yaw in degrees, the
lens and image parameters
The xpos,ypos is from the top-left of the image
The height is in meters
The yaw is from grid north. Positive yaw is clockwise
The roll is from horiznotal. Positive roll is down on the right
The pitch is from horiznotal. Positive pitch is up in the front
return result is a tuple, with meters east and north of current GPS position
'''
from numpy import array, eye
from cuav.uav.uav import uavxfer
from math import pi
xfer = uavxfer()
xfer.setCameraMatrix(C.K)
xfer.setCameraOrientation(0.0, 0.0, pi / 2)
xfer.setFlatEarth(0)
xfer.setPlatformPose(0, 0, -height, math.radians(roll),
math.radians(pitch), math.radians(yaw))
# compute the undistorted points for the ideal camera matrix
src = cv.CreateMat(1, 1, cv.CV_64FC2)
src[0, 0] = (xpos, ypos)
dst = cv.CreateMat(1, 1, cv.CV_64FC2)
R = cv.fromarray(eye(3))
K = cv.fromarray(C.K)
D = cv.fromarray(C.D)
cv.UndistortPoints(src, dst, K, D, R, K)
x = dst[0, 0][0]
y = dst[0, 0][1]
#print '(', xpos,',', ypos,') -> (', x, ',', y, ')'
# negative scale means camera pointing above horizon
# large scale means a long way away also unreliable
(joe_w, scale) = xfer.imageToWorld(x, y)
if (scale < 0 or scale > 500):
return None
#(te, tn) = pixel_position_tridge(xpos, ypos, height, pitch, roll, yaw, lens, sensorwidth, xresolution, yresolution)
#diff = (te-joe_w[1], tn-joe_w[0])
#print 'diff: ', diff
# east and north
return (joe_w[1], joe_w[0])
def rectifyPoint(self, uv_raw):
"""
:param uv_raw: pixel coordinates
:type uv_raw: (u, v)
Applies the rectification specified by camera parameters
:math:`K` and and :math:`D` to point (u, v) and returns the
pixel coordinates of the rectified point.
"""
src = mkmat(1, 2, list(uv_raw))
src = cv.Reshape(src, 2)
dst = cv.CloneMat(src)
cv.UndistortPoints(src, dst, self.K, self.D, self.R, self.P)
return dst[0, 0]
def calculateDistortion(self, image):
size = image.shape[1], image.shape[0]
corners = self.findCorners(image)
self._corners = corners
if corners is None:
return None
patternPoints = np.zeros((np.prod(self.boardSize), 3), np.float32)
patternPoints[:, :2] = np.indices(self.boardSize).T.reshape(-1, 2)
imagePoints = np.array([corners.reshape(-1, 2)])
objectPoints = np.array([patternPoints])
cameraMatrix = np.zeros((3, 3))
distCoefs = np.zeros(4)
rc, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(
objectPoints, imagePoints, self.boardSize, cameraMatrix, distCoefs)
newCameraMatrix, newExtents = cv2.getOptimalNewCameraMatrix(
cameraMatrix, distCoeffs, size, 0.0)
#
# Calculate transformed corners (corners as they would be after the image went through distortion correction)
#
# Need to use a nasty old cv binding to do this since the function went missing in the new cv2 bindings
dest = cv.fromarray(corners.copy())
cv.UndistortPoints(
cv.fromarray(corners),
dest,
cv.fromarray(cameraMatrix),
cv.fromarray(distCoeffs),
P=cv.fromarray(newCameraMatrix),
)
self.distCoeffs = distCoeffs
self.cameraMatrix = cameraMatrix
self.newCameraMatrix = newCameraMatrix
undistortedCorners = np.array(dest)
return undistortedCorners
def get_measurement(self):
"""
Get the target's pixel coordinates as measured by the actual sensor
"""
cm = cv.CreateMat(3, 3, cv.CV_32F)
cv.Set(cm, 0)
cm_t = reshape(matrix(self._camera_matrix, float64), (3, 3))[:3, :3]
for x in range(3):
for y in range(3):
cm[x, y] = cm_t[x, y]
dc = cv.CreateMat(5, 1, cv.CV_32F)
for i in range(len(self._distortion)):
dc[i, 0] = self._distortion[i]
camera_pix = cv.fromarray(
float64([[[pt.x, pt.y]] for pt in self._M_cam.image_points]))
dst = cv.CreateMat(camera_pix.rows, camera_pix.cols, camera_pix.type)
cv.UndistortPoints(camera_pix, dst, cm, dc, P=cm)
return asarray(dst)
def InitPredistortMap(cameraMatrix, distCoeffs, map1, map2):
rows = map1.height
cols = map1.width
mat = cv.CreateMat(1, rows * cols, cv.CV_32FC2)
for row in range(rows):
for col in range(cols):
mat[0, row * cols + col] = (col, row)
R = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.SetIdentity(R)
P = cv.CreateMat(3, 4, cv.CV_64FC1)
cv.SetZero(P)
cv.Copy(cameraMatrix, cv.GetSubRect(P, (0, 0, 3, 3)))
cv.UndistortPoints(mat, mat, cameraMatrix, distCoeffs, R, P)
mat = cv.Reshape(mat, 2, rows)
cv.Split(mat, map1, map2, None, None)
def capture(self):
blank = self.get_picture_of_projection(self.blank_projection)
positive = self.get_picture_of_projection(
self.positive_chessboard_projection)
negative = self.get_picture_of_projection(
self.negative_chessboard_projection)
difference = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.Sub(positive, negative, difference)
cv.NamedWindow("blank", flags=0)
cv.ShowImage("blank", blank)
cv.WaitKey(300)
cv.NamedWindow("difference", flags=0)
cv.ShowImage("difference", difference)
cv.WaitKey(300)
camera_image_points, camera_object_points = detect_chessboard(
blank, self.printed_chessboard_corners_x,
self.printed_chessboard_corners_y, self.printed_chessboard_spacing,
self.printed_chessboard_spacing)
if camera_image_points is None:
return False
cv.UndistortPoints(camera_image_points, camera_image_points,
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs())
homography = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.FindHomography(camera_image_points, camera_object_points,
homography)
object_points, dummy = detect_chessboard(
difference, self.projected_chessboard_corners_x,
self.projected_chessboard_corners_y, None, None)
if object_points is None:
return False
cv.UndistortPoints(object_points, object_points,
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs())
cv.PerspectiveTransform(object_points, object_points, homography)
object_points_3d = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC3)
x = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
y = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
cv.Split(object_points, x, y, None, None)
z = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
cv.SetZero(z)
cv.Merge(x, y, z, None, object_points_3d)
self.object_points.append(object_points_3d)
self.image_points.append(self.get_scene_image_points())
self.number_of_scenes += 1
return True
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_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
