def __init__(self): self.targetLocation = PositionVector() self.vehicleLocation = PositionVector() self.backgroundColor = (74,88,109) #define camera self.camera_width = VN_config.get_integer('camera', 'width', 640) self.camera_height = VN_config.get_integer('camera', 'height', 640) self.camera_vfov = VN_config.get_float('camera', 'vertical-fov',72.42 ) self.camera_hfov = VN_config.get_float('camera', 'horizontal-fov', 72.42) self.camera_fov = math.sqrt(self.camera_vfov**2 + self.camera_hfov**2) self.camera_frameRate = 30
def __init__(self): self.targetLocation = PositionVector() self.vehicleLocation = PositionVector() self.backgroundColor = (74, 88, 109) #define camera self.camera_width = VN_config.get_integer('camera', 'width', 640) self.camera_height = VN_config.get_integer('camera', 'height', 640) self.camera_vfov = VN_config.get_float('camera', 'vertical-fov', 72.42) self.camera_hfov = VN_config.get_float('camera', 'horizontal-fov', 72.42) self.camera_fov = math.sqrt(self.camera_vfov**2 + self.camera_hfov**2) self.camera_frameRate = 30
def __init__(self): self.targetLocation = PositionVector() self.vehicleLocation = PositionVector() self.backgroundColor = (74,88,109) #load target filename = VN_config.get_string('simulator', 'target_location', '../../visnav/target.jpg') target_size = VN_config.get_float('algorithm', 'outer_ring', 1.0) self.load_target(filename,target_size) #define camera self.camera_width = VN_config.get_integer('camera', 'width', 640) self.camera_height = VN_config.get_integer('camera', 'height', 640) self.camera_vfov = VN_config.get_float('camera', 'vertical-fov',72.42 ) self.camera_hfov = VN_config.get_float('camera', 'horizontal-fov', 72.42) self.camera_fov = math.sqrt(self.camera_vfov**2 + self.camera_hfov**2) self.camera_frameRate = 30
def __init__(self): self.targetLocation = PositionVector() self.vehicleLocation = PositionVector() self.backgroundColor = (74, 88, 109) #load target filename = VN_config.get_string('simulator', 'target_location', '../../visnav/target.jpg') target_size = VN_config.get_float('algorithm', 'outer_ring', 1.0) self.load_target(filename, target_size) #define camera self.camera_width = VN_config.get_integer('camera', 'width', 640) self.camera_height = VN_config.get_integer('camera', 'height', 640) self.camera_vfov = VN_config.get_float('camera', 'vertical-fov', 72.42) self.camera_hfov = VN_config.get_float('camera', 'horizontal-fov', 72.42) self.camera_fov = math.sqrt(self.camera_vfov**2 + self.camera_hfov**2) self.camera_frameRate = 30
class PrecisionLandSimulator(): def __init__(self): self.targetLocation = PositionVector() self.vehicleLocation = PositionVector() self.backgroundColor = (74,88,109) #load target filename = VN_config.get_string('simulator', 'target_location', '../../visnav/target.jpg') target_size = VN_config.get_float('algorithm', 'outer_ring', 1.0) self.load_target(filename,target_size) #define camera self.camera_width = VN_config.get_integer('camera', 'width', 640) self.camera_height = VN_config.get_integer('camera', 'height', 640) self.camera_vfov = VN_config.get_float('camera', 'vertical-fov',72.42 ) self.camera_hfov = VN_config.get_float('camera', 'horizontal-fov', 72.42) self.camera_fov = math.sqrt(self.camera_vfov**2 + self.camera_hfov**2) self.camera_frameRate = 30 #load_target- load an image to simulate the target. Enter the actaul target size in meters(assuming the target is square) def load_target(self,filename, actualSize): self.target = cv2.imread(filename) self.target_width = self.target.shape[1] self.target_height = self.target.shape[0] self.actualSize = actualSize #scaling factor for real dimensions to simultor pixels self.pixels_per_meter = (self.target_height + self.target_width) / (2.0 * actualSize) #set_target_location- give the target a gps location def set_target_location(self, location): self.targetLocation.set_from_location(location) #refresh_simulator - update vehicle position info necessary to simulate an image def refresh_simulator(self, vehicleLocation, vehicleAttitude): #get gps location of vehicle self.vehicleLocation.set_from_location(vehicleLocation) self.vehicleAttitude = vehicleAttitude #main - code used to test the simulator. Must be run from sitl. Control vehicle in guided mode using arrow keys,r,t,q,e def main(self): veh_control.connect(local_connect()) self.set_target_location(veh_control.get_location()) while(veh_control.is_connected()): location = veh_control.get_location() attitude = veh_control.get_attitude() self.refresh_simulator(location,attitude) frame = self.get_frame() cv2.imshow('frame',frame) key = cv2.waitKey(1) print key if key ==1113938: veh_control.set_velocity(2,0,0) #forward elif key == 1113940: veh_control.set_velocity(-2,0,0) #backward elif key == 1113937: veh_control.set_velocity(0,-2,0) #left elif key ==1113939: veh_control.set_velocity(0,2,0) #right elif(key == 1048690): yaw = math.degrees(attitude.yaw) #yaw left veh_control.set_yaw(yaw - 5) elif(key == 1048692): yaw = math.degrees(attitude.yaw) #yaw right veh_control.set_yaw(yaw + 5) elif(key == 1048677): veh_control.set_velocity(0,0,-2) #down elif(key == 1048689): veh_control.set_velocity(0,0,2) #up else: veh_control.set_velocity(0,0,0) #still #project_3D_to_2D - project a 3d point onto a 2d plane. Covert from world perspective to camera perspective def project_3D_to_2D(self,thetaX,thetaY,thetaZ, aX, aY,aZ, cX, cY, cZ, height, width, fov): dX = math.cos(-thetaY) * (math.sin(-thetaZ)*(cY-aY) + math.cos(-thetaZ)*(cX-aX)) - math.sin(-thetaY)*(aZ-cZ) dY = math.sin(-thetaX) * (math.cos(-thetaY)*(aZ-cZ) + math.sin(-thetaY)*(math.sin(-thetaZ)*(cY-aY) + math.cos(-thetaZ)*(cX-aX))) + math.cos(-thetaX)*(math.cos(-thetaZ)*(cY-aY) - math.sin(-thetaZ) * (cX-aX)) dZ = math.cos(-thetaX) * (math.cos(-thetaY)*(aZ-cZ) + math.sin(-thetaY)*(math.sin(-thetaZ)*(cY-aY) + math.cos(-thetaZ)*(cX-aX))) - math.sin(-thetaX)*(math.cos(-thetaZ)*(cY-aY) - math.sin(-thetaZ) * (cX-aX)) #veiwer position eX = 0 eY = 0 eZ = 1.0/math.tan(math.radians(fov)/2.0) #2D point bX = (dX - eX)*(eZ/dZ) bY = (dY - eY)*(eZ/dZ) #scaled to resolution sX = bX * width sY = bY * height return (sX,sY) #simulate_target - simulate an image given the target position[aX,aY,aZ](pixels)and camera position[cX,cY,cZ](pixels) and camera orientation def simulate_target(self,thetaX,thetaY,thetaZ, aX, aY, aZ, cX, cY, cZ, camera_height, camera_width, fov): img_width = self.target_width img_height = self.target_height #point maps corners = np.float32([[-img_width/2,img_height/2],[img_width/2 ,img_height/2],[-img_width/2,-img_height/2],[img_width/2, -img_height/2]]) newCorners = np.float32([[0,0],[0,0],[0,0],[0,0]]) #calculate projection for four corners of image for i in range(0,len(corners)): #shift to world x = corners[i][0] + cX - img_width/2.0 y = corners[i][1] + cY - img_height/2.0 #calculate perspective and position x , y = self.project_3D_to_2D(thetaX,thetaY,thetaZ, aY, aX, aZ, y, x, cZ,camera_height,camera_width,fov) #shift to camera x , y = shift_to_image((x,y),camera_width,camera_height) newCorners[i] = x,y #project image M = cv2.getPerspectiveTransform(corners,newCorners) sim = cv2.warpPerspective(self.target,M,(self.camera_width,self.camera_height),borderValue=self.backgroundColor) return sim #get_frame - retreive a simulated camera image def get_frame(self): start = current_milli_time() #distance bewteen camera and target in meters aX,aY,aZ = self.targetLocation.x, self.targetLocation.y, self.targetLocation.z cX,cY,cZ = self.vehicleLocation.x, self.vehicleLocation.y, self.vehicleLocation.z #camera angle thetaX = self.vehicleAttitude.pitch thetaY = self.vehicleAttitude.roll thetaZ = self.vehicleAttitude.yaw #convert distance bewtween camera and target in pixels aX = aX * self.pixels_per_meter aY = aY * self.pixels_per_meter aZ = aZ * self.pixels_per_meter cX = cX * self.pixels_per_meter cY = cY * self.pixels_per_meter cZ = cZ * self.pixels_per_meter #render image sim = self.simulate_target(thetaX,thetaY,thetaZ, aX, aY, aZ, cX, cY, cZ, self.camera_height, self.camera_width, self.camera_fov) #simulate framerate while(1000/self.camera_frameRate > current_milli_time() - start): pass return sim
class PrecisionLandSimulator(): def __init__(self): self.targetLocation = PositionVector() self.vehicleLocation = PositionVector() self.backgroundColor = (74, 88, 109) #define camera self.camera_width = VN_config.get_integer('camera', 'width', 640) self.camera_height = VN_config.get_integer('camera', 'height', 640) self.camera_vfov = VN_config.get_float('camera', 'vertical-fov', 72.42) self.camera_hfov = VN_config.get_float('camera', 'horizontal-fov', 72.42) self.camera_fov = math.sqrt(self.camera_vfov**2 + self.camera_hfov**2) self.camera_frameRate = 30 #load_target- load an image to simulate the target. Enter the actaul target size in meters(assuming the target is square) def load_target(self, filename, actualSize): self.target = cv2.imread(filename) if self.target is None: print "Unable to load target image!!!!!!!!!!!!\nExiting PrecisionLand.py" sys.exit(0) self.target_width = self.target.shape[1] self.target_height = self.target.shape[0] self.actualSize = actualSize #scaling factor for real dimensions to simultor pixels self.pixels_per_meter = (self.target_height + self.target_width) / (2.0 * actualSize) #set_target_location- give the target a gps location def set_target_location(self, location): self.targetLocation.set_from_location(location) #refresh_simulator - update vehicle position info necessary to simulate an image def refresh_simulator(self, vehicleLocation, vehicleAttitude): #get gps location of vehicle self.vehicleLocation.set_from_location(vehicleLocation) self.vehicleAttitude = vehicleAttitude #main - code used to test the simulator. Must be run from sitl. Control vehicle in guided mode using arrow keys,r,t,q,e def main(self): veh_control.connect(local_connect()) self.set_target_location(veh_control.get_location()) while (veh_control.is_connected()): location = veh_control.get_location() attitude = veh_control.get_attitude() self.refresh_simulator(location, attitude) frame = self.get_frame() cv2.imshow('frame', frame) key = cv2.waitKey(1) print key if key == 1113938: veh_control.set_velocity(2, 0, 0) #forward elif key == 1113940: veh_control.set_velocity(-2, 0, 0) #backward elif key == 1113937: veh_control.set_velocity(0, -2, 0) #left elif key == 1113939: veh_control.set_velocity(0, 2, 0) #right elif (key == 1048690): yaw = math.degrees(attitude.yaw) #yaw left veh_control.set_yaw(yaw - 5) elif (key == 1048692): yaw = math.degrees(attitude.yaw) #yaw right veh_control.set_yaw(yaw + 5) elif (key == 1048677): veh_control.set_velocity(0, 0, -2) #down elif (key == 1048689): veh_control.set_velocity(0, 0, 2) #up else: veh_control.set_velocity(0, 0, 0) #still #project_3D_to_2D - project a 3d point onto a 2d plane. Covert from world perspective to camera perspective def project_3D_to_2D(self, thetaX, thetaY, thetaZ, aX, aY, aZ, cX, cY, cZ, height, width, fov): dX = math.cos(-thetaY) * (math.sin(-thetaZ) * (cY - aY) + math.cos(-thetaZ) * (cX - aX)) - math.sin(-thetaY) * (aZ - cZ) dY = math.sin(-thetaX) * ( math.cos(-thetaY) * (aZ - cZ) + math.sin(-thetaY) * (math.sin(-thetaZ) * (cY - aY) + math.cos(-thetaZ) * (cX - aX))) + math.cos(-thetaX) * (math.cos(-thetaZ) * (cY - aY) - math.sin(-thetaZ) * (cX - aX)) dZ = math.cos(-thetaX) * ( math.cos(-thetaY) * (aZ - cZ) + math.sin(-thetaY) * (math.sin(-thetaZ) * (cY - aY) + math.cos(-thetaZ) * (cX - aX))) - math.sin(-thetaX) * (math.cos(-thetaZ) * (cY - aY) - math.sin(-thetaZ) * (cX - aX)) #veiwer position eX = 0 eY = 0 eZ = 1.0 / math.tan(math.radians(fov) / 2.0) #2D point bX = (dX - eX) * (eZ / dZ) bY = (dY - eY) * (eZ / dZ) #scaled to resolution sX = bX * width sY = bY * height return (sX, sY) #simulate_target - simulate an image given the target position[aX,aY,aZ](pixels)and camera position[cX,cY,cZ](pixels) and camera orientation def simulate_target(self, thetaX, thetaY, thetaZ, aX, aY, aZ, cX, cY, cZ, camera_height, camera_width, fov): img_width = self.target_width img_height = self.target_height #point maps corners = np.float32([[-img_width / 2, img_height / 2], [img_width / 2, img_height / 2], [-img_width / 2, -img_height / 2], [img_width / 2, -img_height / 2]]) newCorners = np.float32([[0, 0], [0, 0], [0, 0], [0, 0]]) #calculate projection for four corners of image for i in range(0, len(corners)): #shift to world x = corners[i][0] + cX - img_width / 2.0 y = corners[i][1] + cY - img_height / 2.0 #calculate perspective and position x, y = self.project_3D_to_2D(thetaX, thetaY, thetaZ, aY, aX, aZ, y, x, cZ, camera_height, camera_width, fov) #shift to camera x, y = shift_to_image((x, y), camera_width, camera_height) newCorners[i] = x, y #project image M = cv2.getPerspectiveTransform(corners, newCorners) sim = cv2.warpPerspective(self.target, M, (self.camera_width, self.camera_height), borderValue=self.backgroundColor) return sim #get_frame - retreive a simulated camera image def get_frame(self): start = current_milli_time() #distance bewteen camera and target in meters aX, aY, aZ = self.targetLocation.x, self.targetLocation.y, self.targetLocation.z cX, cY, cZ = self.vehicleLocation.x, self.vehicleLocation.y, self.vehicleLocation.z #camera angle thetaX = self.vehicleAttitude.pitch thetaY = self.vehicleAttitude.roll thetaZ = self.vehicleAttitude.yaw #convert distance bewtween camera and target in pixels aX = aX * self.pixels_per_meter aY = aY * self.pixels_per_meter aZ = aZ * self.pixels_per_meter cX = cX * self.pixels_per_meter cY = cY * self.pixels_per_meter cZ = cZ * self.pixels_per_meter #render image sim = self.simulate_target(thetaX, thetaY, thetaZ, aX, aY, aZ, cX, cY, cZ, self.camera_height, self.camera_width, self.camera_fov) #simulate framerate while (1000 / self.camera_frameRate > current_milli_time() - start): pass sim = cv2.cvtColor(sim, cv2.COLOR_BGR2GRAY) return sim