import math
import os
import hypothesis
import hypothesis.strategies as st
import numpy as np
from kitcar_utils.camera.camera_specs import CameraSpecs
from kitcar_utils.geometry import Vector
from ..sample_generator import SampleGenerator
[docs]@hypothesis.given(
min_dist=st.floats(min_value=0, max_value=1e10),
max_dist=st.floats(min_value=1e-10, max_value=1e10),
)
def test_position_gen(min_dist, max_dist, camera_specs, ts_sizes):
min_dist, max_dist = (
(min_dist, max_dist) if min_dist <= max_dist else (max_dist, min_dist)
)
sample_gen = SampleGenerator(camera_specs, ts_sizes, min_dist, max_dist)
camera_specs = CameraSpecs.from_yaml(camera_specs)
rand_vec = Vector(sample_gen.sample_world_vec())
assert (
round(min_dist, 2) <= round(abs(rand_vec), 2) <= round(max_dist, 2)
), f"Failure: {min_dist, abs(rand_vec), max_dist}"
assert (
math.acos(rand_vec * Vector(1, 0, 0) / abs(rand_vec))
<= camera_specs.horizontal_fov / 2
)
[docs]@hypothesis.given(
height=st.floats(min_value=0, max_value=2e-1),
)
def test_tf_to_img(height, camera_specs, ts_sizes):
sample_gen = SampleGenerator(camera_specs, ts_sizes, 1, 3)
camera_specs = CameraSpecs.from_yaml(camera_specs)
vec_ground = sample_gen.sample_world_vec()
# vec_ground = Vector(1, 1, 0)
# 1. ground point
img = sample_gen.vehicle_point_to_img(vec_ground)
img_inv = Vector(np.array(np.linalg.inv(camera_specs.H) @ img)[0])
img_inv = 1 / img_inv.z * img_inv
exp_vec = Vector(*vec_ground.xy, 1)
assert exp_vec == img_inv
# 2. non ground ---> direction
vec = vec_ground + Vector(0, 0, height)
img = sample_gen.vehicle_point_to_img(vec)
img_inv = Vector(np.array(np.linalg.inv(camera_specs.H) @ img)[0])
img_inv = 1 / img_inv.z * img_inv
img_inv = Vector(*img_inv.xy, 0)
assert abs(1 / abs(img_inv) * 1 / abs(vec_ground) * img_inv.cross(vec_ground)) < 0.01, (
"Vectors do not point in the same direction:"
f" {1/abs(img_inv)*img_inv}, {1/abs(vec_ground)*vec_ground}"
)
assert round(abs(img_inv), 5) >= round(
abs(vec_ground), 5
), f"Vector should be longer. {abs(img_inv)} vs {abs(vec_ground)}"
[docs]def main():
ts_sizes = os.path.join(os.path.dirname(__file__), "../ts_sizes.yaml")
camera = os.path.join(os.path.dirname(__file__), "../camera.yaml")
test_position_gen(camera_specs=camera, ts_sizes=ts_sizes)
test_tf_to_img(camera_specs=camera, ts_sizes=ts_sizes)
if __name__ == "__main__":
main()