# ISC License
#
# Copyright (c) 2023, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
#
# Permission to use, copy, modify, and/or distribute this software for any
# purpose with or without fee is hereby granted, provided that the above
# copyright notice and this permission notice appear in all copies.
#
# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
import os, inspect
import numpy as np
import pytest
from matplotlib import pyplot as plt
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.utilities import RigidBodyKinematics as rbk
from Basilisk.utilities import orbitalMotion
from Basilisk.utilities import simIncludeGravBody
from Basilisk.architecture import messaging
from Basilisk.simulation import pinholeCamera
from Basilisk.simulation import spacecraft
from Basilisk.utilities import unitTestSupport
from Basilisk import __path__
filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))
bskName = 'Basilisk'
splitPath = path.split(bskName)
bskPath = __path__[0]
[docs]def test_visibility():
"""
Tests whether pinholeCamera:
1. Computes correctly pixels for center and corners.
2. Detects correctly that some landmarks are not within field of view.
:return:
"""
simTime = 1.
simTaskName = "simTask"
simProcessName = "simProcess"
scSim = SimulationBaseClass.SimBaseClass()
dynProcess = scSim.CreateNewProcess(simProcessName)
simulationTime = macros.sec2nano(simTime)
simulationTimeStep = macros.sec2nano(1.)
dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep))
# Set the landmarks distribution to be tested
# The first five landmarks shall correspond to center and corners of the image (for the prescribed situation)
# The last five landmarks shall be detected as not within FOV
FOVx = 38.17124015837933 * np.pi/180 # this is the horizontal FOV, check camera.FOVx
FOVy = 29.094758030219015 * np.pi/180 # this is the vertical FOV, check camera.FOVy
n_lmk = 10
pos_lmk = np.array([[-16*1e3, 0, 0], # Image center
[-16*1e3, 18*1e3 * np.tan(FOVy/2), 0], # Image corner (18 km = horizontal distance sc-lmk)
[-16*1e3, -18*1e3*np.tan(FOVy/2), 0], # Image corner
[-16*1e3, 0, 18*1e3*np.tan(FOVx/2)], # Image corner
[-16*1e3, 0, -18*1e3*np.tan(FOVx/2)], # Image corner
[16*1e3, 0, 0], # Not visible
[0, 16*1e3, 0], # Not visible
[0, -16*1e3, 0], # Not visible
[0, 0, 16*1e3], # Not visible
[0, 0, -16*1e3]]) # Not visible
normal_lmk = np.array([[-1, 0, 0],
[-1, 0, 0], # Mock to ensure visibility
[-1, 0, 0], # Mock to ensure visibility
[-1, 0, 0], # Mock to ensure visibility
[-1, 0, 0], # Mock to ensure visibility
[1, 0, 0],
[0, 1, 0],
[0, -1, 0],
[0, 0, 1],
[0, 0, -1]])
# Set the pinhole camera module
camera = pinholeCamera.PinholeCamera()
camera.f = 25*1e-3
camera.nxPixel = 2048
camera.nyPixel = 1536
camera.wPixel = (17.3*1e-3) / 2048
dcm_CB = np.array([[0, 0, -1],
[0, 1, 0],
[1, 0, 0]])
camera.dcm_CB = dcm_CB.tolist()
for i in range(n_lmk):
camera.addLandmark(pos_lmk[i, 0:3], normal_lmk[i, 0:3])
scSim.AddModelToTask(simTaskName, camera)
# Write out mock planet ephemeris message
planet_message = messaging.EphemerisMsgPayload()
aP = 1.4583 * 149597870.7*1e3
r_PN_N = np.array([aP, 0, 0])
v_PN_N = np.array([0, np.sqrt(orbitalMotion.MU_SUN/aP), 0])
dcm_PN = np.identity(3)
mrp_PN = rbk.C2MRP(dcm_PN)
planet_message.r_BdyZero_N = r_PN_N
planet_message.v_BdyZero_N = v_PN_N
planet_message.sigma_BN = mrp_PN
planetMsg = messaging.EphemerisMsg().write(planet_message)
camera.ephemerisInMsg.subscribeTo(planetMsg)
# Write out mock spacecraft message (pointing towards planet shall be ensured)
a = 34 * 1e3
mu = 4.4631 * 1e5
r_BP_P = np.array([-a, 0, 0])
v_BP_P = np.array([0, -np.sqrt(mu/a), 0])
r_BN_N = r_BP_P + r_PN_N
v_BN_N = v_BP_P + v_PN_N
ir = r_BP_P / np.linalg.norm(r_BP_P)
ih = np.cross(ir, v_BP_P/np.linalg.norm(v_BP_P))
it = np.cross(ir, ih)
dcm_BP = np.zeros((3, 3))
dcm_BP[0:3, 0] = -ir
dcm_BP[0:3, 1] = ih
dcm_BP[0:3, 2] = it
dcm_BN = np.matmul(dcm_BP, dcm_PN)
mrp_BN = rbk.C2MRP(dcm_BN)
sc1_message = messaging.SCStatesMsgPayload()
sc1_message.r_BN_N = r_BN_N
sc1_message.v_BN_N = v_BN_N
sc1_message.sigma_BN = mrp_BN
scMsg = messaging.SCStatesMsg().write(sc1_message)
camera.scStateInMsg.subscribeTo(scMsg)
# Log the landmark messages
numDataPoints = 2
samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints)
dataLog = []
for i in range(len(pos_lmk)):
dataLog.append(camera.landmarkOutMsgs[i].recorder(samplingTime))
scSim.AddModelToTask(simTaskName, dataLog[i])
# Run the sim
scSim.InitializeSimulation()
scSim.ConfigureStopTime(simulationTime)
scSim.ExecuteSimulation()
# Get the logged data
isvisibleLmk = np.zeros((2, n_lmk))
pixelLmk = np.zeros((2, n_lmk, 2))
for i in range(n_lmk):
isvisibleLmk[:, i] = dataLog[i].isVisible
pixelLmk[:, i, 0:2] = dataLog[i].pL
# Define expected values
# The corners shall correspond to the maximum pixel resolution
# The default behaviour for a point lying in the origin is +1
accuracy = 1e-8
ref_pixel = np.array([[1, 1],
[1, 1536/2],
[1, -1536/2],
[-2048/2, 1],
[2048/2, 1],
[0, 0],
[0, 0],
[0, 0],
[0, 0],
[0, 0]])
ref_isvisible = np.array([1, 1, 1, 1, 1, 0, 0, 0, 0, 0])
# Compare to expected values
pixel_worked = pixelLmk[0, :, :] == pytest.approx(ref_pixel, accuracy)
isvisible_worked = isvisibleLmk[0, :] == pytest.approx(ref_isvisible, accuracy)
assert (pixel_worked and isvisible_worked)
if __name__ == '__main__':
test_visibility()