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