40#define ANGLE_BFP_SCALE 4096.0
41#define SPEED_BFP_SCALE 524288.0
42#define POSITION_BFP_SCALE 256.0
43#define RAD_TO_DEG 57.29577951308232
44#define JPEG_COMPONENTS 3U
45#define JPEG_QUALITY 95
46#define FOCAL_LENGTH_FACTOR 0.9
47#define PROJECTION_EPSILON 1e-9
48#define PROJECTION_MIN_DEPTH 1e-4
49#define PROJECTION_FOV_SAMPLES 192
50#define PROJECTION_REFINE_STEPS 48
51#define PROJECTION_MIN_RAY_SCALE 1e-4
52#define COVER_SAFETY_FACTOR 0.999
53#define IMAGE_MOCK_MAX_DIMENSION 8192U
54#define IMAGE_MOCK_MAX_PIXELS (2028U * 1520U)
55#define IMAGE_MOCK_RGB_CAPACITY ((size_t)IMAGE_MOCK_MAX_PIXELS * JPEG_COMPONENTS)
56#define MAP_TILE_SIZE 256U
57#define MAP_MOSAIC_DIMENSION (3U * MAP_TILE_SIZE)
58#define MAP_MOSAIC_CAPACITY ((size_t)MAP_MOSAIC_DIMENSION * MAP_MOSAIC_DIMENSION * JPEG_COMPONENTS)
60#ifndef near_horizon_case_black
61 #define near_horizon_case_black 0
64#ifndef near_horizon_roll_deg
65 #define near_horizon_roll_deg 75.0
68#ifndef near_horizon_pitch_deg
69 #define near_horizon_pitch_deg 75.0
72#ifndef near_horizon_combined_tilt_deg
73 #define near_horizon_combined_tilt_deg 70.0
76#if near_horizon_case_black != 0 && near_horizon_case_black != 1
77 #error "near_horizon_case_black must be 0 or 1"
90 void (*
emit)(j_common_ptr, int);
119 double roll_rad,
double pitch_rad,
double yaw_rad,
120 bool synthesize_terrain);
121static void build_rotation(
double roll_rad,
double pitch_rad,
double yaw_rad,
122 double rotation[3][3]);
123static bool project_point(
double rotation[3][3],
double focal_length,
124 double center_x,
double center_y,
double ray_scale,
125 double destination_x,
double destination_y,
126 double *source_x,
double *source_y);
128 double rotation[3][3],
double focal_length,
129 double center_x,
double center_y,
double ray_scale);
131 double rotation[3][3],
double focal_length,
132 double center_x,
double center_y,
double ray_scale,
135 double rotation[3][3],
double focal_length,
136 double center_x,
double center_y,
145 double horizontal_fov_deg,
double vertical_fov_deg);
162static bool map_project(
double rotation[3][3],
double forward,
double right,
163 double altitude_pixels,
double center_x,
double center_y,
164 double *map_x,
double *map_y)
166 double north = rotation[0][0] * forward + rotation[0][1] * right + rotation[0][2];
167 double east = rotation[1][0] * forward + rotation[1][1] * right + rotation[1][2];
168 double down = rotation[2][0] * forward + rotation[2][1] * right + rotation[2][2];
170 *map_x = center_x + altitude_pixels * east / down;
171 *map_y = center_y - altitude_pixels * north / down;
172 return isfinite(*map_x) && isfinite(*map_y);
188 double horizontal_fov_deg,
double vertical_fov_deg)
191 if (lock_result != 0) {
196 horizontal_fov_deg, vertical_fov_deg);
217 double horizontal_fov_deg,
double vertical_fov_deg)
220 const int tile_count = 1 << 18;
221 const double earth_radius = 6378137.0;
222 const double pi = 3.14159265358979323846;
223 if (filename == NULL || cache_directory == NULL || cache_directory[0] ==
'\0'
225 || !isfinite(horizontal_fov_deg) || !isfinite(vertical_fov_deg)
226 || horizontal_fov_deg <= 0.0 || horizontal_fov_deg >= 179.0
227 || vertical_fov_deg <= 0.0 || vertical_fov_deg >= 179.0
228 ||
shot->data.groundalt <= 0 || fabs(
shot->data.lat / 1e7) > 85.05112878
229 || fabs(
shot->data.lon / 1e7) > 180.0) {
230 fprintf(stderr,
"MOCK_MAP:\tinvalid pose, positive AGL, image size, or FOV required\n");
235 double world_pixels = (double)tile_size * tile_count;
236 double center_x = (
shot->data.lon / 1e7 + 180.0) / 360.0 * world_pixels;
237 double center_y = (1.0 - asinh(tan(
latitude)) / pi) * 0.5 * world_pixels;
240 double meters_per_pixel = 2.0 * pi * earth_radius * cos(
latitude) / world_pixels;
242 double half_horizontal = tan(horizontal_fov_deg /
RAD_TO_DEG * 0.5);
243 double half_vertical = tan(vertical_fov_deg /
RAD_TO_DEG * 0.5);
244 double rotation[3][3];
247 double minimum_x = INFINITY, minimum_y = INFINITY;
248 double maximum_x = -INFINITY, maximum_y = -INFINITY;
250 for (
int column = -1; column <= 1; column += 2) {
252 if (!
map_project(rotation, -
row * half_vertical, column * half_horizontal,
253 altitude_pixels, center_x, center_y, &map_x, &map_y)) {
254 fprintf(stderr,
"MOCK_MAP:\tcamera footprint reaches the horizon\n");
258 minimum_x = fmin(minimum_x, map_x);
259 minimum_y = fmin(minimum_y, map_y);
260 maximum_x = fmax(maximum_x, map_x);
261 maximum_y = fmax(maximum_y, map_y);
264 int center_tile_x = (int)floor(center_x / tile_size);
265 int center_tile_y = (int)floor(center_y / tile_size);
266 if (minimum_x < (center_tile_x - 1.0) * tile_size
267 || minimum_y < (center_tile_y - 1.0) * tile_size
268 || ceil(maximum_x) >= (center_tile_x + 2.0) * tile_size
269 || ceil(maximum_y) >= (center_tile_y + 2.0) * tile_size
270 || minimum_y < 0.0 || ceil(maximum_y) >= world_pixels) {
271 fprintf(stderr,
"MOCK_MAP:\tfootprint exceeds the zoom-18 XYZ tile 3x3 neighborhood\n");
275 int first_x = (int)floor(minimum_x / tile_size);
276 int first_y = (int)floor(minimum_y / tile_size);
277 int last_x = (int)floor(ceil(maximum_x) / tile_size);
278 int last_y = (int)floor(ceil(maximum_y) / tile_size);
280 workspace.mosaic, (size_t)(last_x - first_x + 1) * tile_size,
281 (size_t)(last_y - first_y + 1) * tile_size
283 for (
int tile_y = first_y; tile_y <= last_y; ++tile_y) {
284 for (
int tile_x = first_x; tile_x <= last_x; ++tile_x) {
286 int wrapped_x = (tile_x % tile_count + tile_count) % tile_count;
287 int length = snprintf(path,
sizeof(path),
"%s/GOOGLE/18/%d/%d.jpeg",
288 cache_directory, wrapped_x, tile_y);
289 if (length < 0 || (
size_t)length >=
sizeof(path)) {
290 errno = ENAMETOOLONG;
293 FILE *input = fopen(path,
"rb");
296 if (
result == 0 && (tile.
width != (
size_t)tile_size || tile.
height != (
size_t)tile_size)) {
303 if (input != NULL && fclose(input) != 0)
result = -1;
305 fprintf(stderr,
"MOCK_MAP:\tmissing or invalid 256x256 JPEG tile: %s\n", path);
308 size_t offset_x = (size_t)(tile_x - first_x) * tile_size;
309 size_t offset_y = (size_t)(tile_y - first_y) * tile_size;
311 for (
size_t column = 0; column < tile.
width && column + offset_x < mosaic.
width; ++column) {
322 double forward = (1.0 - 2.0 *
row / (
height - 1)) * half_vertical;
323 for (
size_t column = 0; column <
width; ++column) {
324 double right = (2.0 * column / (
width - 1) - 1.0) * half_horizontal;
326 if (!
map_project(rotation, forward, right, altitude_pixels, center_x, center_y, &map_x, &map_y)) {
329 map_x -= (double)first_x * tile_size;
330 map_y -= (double)first_y * tile_size;
331 if (map_x < 0.0 || map_y < 0.0 || map_x > mosaic.
width - 1 || map_y > mosaic.
height - 1) {
339 printf(
"MOCK_MAP:\tXYZ z18 tiles %d..%d/%d..%d | %.7f,%.7f AGL %.2fm FOV %.2fx%.2f | %s\n",
340 first_x, last_x, first_y, last_y,
shot->data.lat / 1e7,
shot->data.lon / 1e7,
349 if (lock_result != 0) {
369 if (filename == NULL ||
shot == NULL) {
380 fprintf(stderr,
"MOCK_TRANSFORM:\tnear-horizon thresholds must be between 0 and 180 degrees\n");
385 double altitude_m =
shot->data.alt / 1000.0;
392 double blur_factor = speed_m_s / 100.0;
395 printf(
"MOCK_TRANSFORM:\talt %.3f m AGL %.3f m | roll %.3f pitch %.3f yaw %.3f deg | "
396 "speed %.3f m/s course %.3f deg | blur factor %.6f (not applied)\n",
397 altitude_m, ground_altitude_m,
399 speed_m_s, course_deg, blur_factor);
402 printf(
"MOCK_TRANSFORM:\tnear-horizon attitude: generating black frame\n");
411 FILE *input = fopen(filename,
"rb");
413 fprintf(stderr,
"MOCK_TRANSFORM:\tfailed to open %s: %s\n", filename, strerror(errno));
419 fprintf(stderr,
"MOCK_TRANSFORM:\tfailed to decode JPEG %s\n", filename);
423 if (fclose(input) != 0) {
450 longjmp(
error->recovery, 1);
464 error->emit(state, level);
477 double absolute_roll_deg = fabs(roll_rad *
RAD_TO_DEG);
478 double absolute_pitch_deg = fabs(pitch_rad *
RAD_TO_DEG);
479 double optical_axis_cosine = cos(roll_rad) * cos(pitch_rad);
480 optical_axis_cosine = fmax(-1.0, fmin(1.0, optical_axis_cosine));
481 double combined_tilt_deg = acos(optical_axis_cosine) *
RAD_TO_DEG;
496 return width >= 2 && height >= 2
518 if (setjmp(
error->recovery) != 0) {
519 jpeg_destroy_decompress(
decoder);
523 jpeg_create_decompress(
decoder);
524 jpeg_stdio_src(
decoder, input);
525 if (jpeg_read_header(
decoder, TRUE) != JPEG_HEADER_OK
527 jpeg_destroy_decompress(
decoder);
533 jpeg_destroy_decompress(
decoder);
555 if (setjmp(
error->recovery) != 0) {
556 jpeg_destroy_decompress(
decoder);
560 jpeg_create_decompress(
decoder);
561 jpeg_stdio_src(
decoder, input);
562 if (jpeg_read_header(
decoder, TRUE) != JPEG_HEADER_OK
564 jpeg_destroy_decompress(
decoder);
568 decoder->out_color_space = JCS_RGB;
569 jpeg_start_decompress(
decoder);
575 jpeg_destroy_decompress(
decoder);
581 if (
image->pixels == NULL) {
586 JSAMPROW
row[1] = {&
image->pixels[(size_t)
decoder->output_scanline * row_size]};
587 if (jpeg_read_scanlines(
decoder,
row, 1) != 1) {
588 jpeg_destroy_decompress(
decoder);
593 jpeg_finish_decompress(
decoder);
594 jpeg_destroy_decompress(
decoder);
617 if (setjmp(
error->recovery) != 0) {
618 jpeg_destroy_compress(
encoder);
627 encoder->in_color_space = JCS_RGB;
630 jpeg_start_compress(
encoder, TRUE);
634 JSAMPROW
row[1] = {(JSAMPROW) &
image->pixels[(
size_t)
encoder->next_scanline * row_size]};
635 if (jpeg_write_scanlines(
encoder,
row, 1) != 1) {
636 jpeg_destroy_compress(
encoder);
642 jpeg_destroy_compress(
encoder);
660 double roll_rad,
double pitch_rad,
double yaw_rad,
661 bool synthesize_terrain)
663 double center_x = ((double)source->
width - 1.0) / 2.0;
664 double center_y = ((double)source->
height - 1.0) / 2.0;
667 double rotation[3][3];
670 synthesize_terrain =
true;
673 if (synthesize_terrain) {
678 printf(
"MOCK_TRANSFORM:\tnear-horizon attitude: synthesizing extended terrain\n");
680 fprintf(stderr,
"MOCK_TRANSFORM:\tattitude is outside the usable single-image camera model\n");
685 || destination->
pixels == NULL) {
689 if (!synthesize_terrain) {
693 printf(
"MOCK_TRANSFORM:\tborderless cover crop: %.3fx zoom, pan %.1f px x %.1f px\n",
694 cover_zoom, pan_x, pan_y);
699 double ray_x_start = -fit.
ray_scale * center_x / focal_length;
700 double ray_x_increment = fit.
ray_scale / focal_length;
701 double source_ray_x_increment = rotation[0][0] * ray_x_increment;
702 double source_ray_y_increment = rotation[0][1] * ray_x_increment;
703 double source_ray_z_increment = rotation[0][2] * ray_x_increment;
705 for (
size_t destination_y = 0; destination_y < destination->
height; destination_y++) {
706 double ray_y = fit.
ray_scale * ((double)destination_y - center_y) / focal_length;
707 double source_ray_x = rotation[0][0] * ray_x_start + rotation[1][0] * ray_y + rotation[2][0];
708 double source_ray_y = rotation[0][1] * ray_x_start + rotation[1][1] * ray_y + rotation[2][1];
709 double source_ray_z = rotation[0][2] * ray_x_start + rotation[1][2] * ray_y + rotation[2][2];
711 for (
size_t destination_x = 0; destination_x < destination->
width; destination_x++) {
712 double projection_depth = source_ray_z;
714 if (!synthesize_terrain) {
717 projection_depth = projection_depth < 0.0
721 double inverse_depth = focal_length / projection_depth;
722 double source_x = source_ray_x * inverse_depth + center_x;
723 double source_y = source_ray_y * inverse_depth + center_y;
727 if (synthesize_terrain) {
731 source_x = fmax(0.0, fmin((
double)(source->
width - 1), source_x));
732 source_y = fmax(0.0, fmin((
double)(source->
height - 1), source_y));
735 size_t destination_offset = (destination_y * destination->
width + destination_x) *
JPEG_COMPONENTS;
737 &destination->
pixels[destination_offset]);
738 source_ray_x += source_ray_x_increment;
739 source_ray_y += source_ray_y_increment;
740 source_ray_z += source_ray_z_increment;
756 double maximum = (double)(extent - 1);
757 double period = 2.0 * maximum;
758 double mirrored = fmod(coordinate, period);
759 if (mirrored < 0.0) {
762 return mirrored <= maximum ? mirrored : period - mirrored;
774 FILE *input = fopen(filename,
"rb");
780 if (fclose(input) != 0) {
803 double rotation[3][3])
805 double cosine_roll = cos(roll_rad);
806 double sine_roll = sin(roll_rad);
807 double cosine_pitch = cos(pitch_rad);
808 double sine_pitch = sin(pitch_rad);
809 double cosine_yaw = cos(yaw_rad);
810 double sine_yaw = sin(yaw_rad);
812 rotation[0][0] = cosine_yaw * cosine_pitch;
813 rotation[0][1] = cosine_yaw * sine_pitch * sine_roll - sine_yaw * cosine_roll;
814 rotation[0][2] = cosine_yaw * sine_pitch * cosine_roll + sine_yaw * sine_roll;
815 rotation[1][0] = sine_yaw * cosine_pitch;
816 rotation[1][1] = sine_yaw * sine_pitch * sine_roll + cosine_yaw * cosine_roll;
817 rotation[1][2] = sine_yaw * sine_pitch * cosine_roll - cosine_yaw * sine_roll;
818 rotation[2][0] = -sine_pitch;
819 rotation[2][1] = cosine_pitch * sine_roll;
820 rotation[2][2] = cosine_pitch * cosine_roll;
839 double center_x,
double center_y,
double ray_scale,
840 double destination_x,
double destination_y,
841 double *source_x,
double *source_y)
843 double ray_x = ray_scale * (destination_x - center_x) / focal_length;
844 double ray_y = ray_scale * (destination_y - center_y) / focal_length;
845 double source_ray_x = rotation[0][0] * ray_x + rotation[1][0] * ray_y + rotation[2][0];
846 double source_ray_y = rotation[0][1] * ray_x + rotation[1][1] * ray_y + rotation[2][1];
847 double source_ray_z = rotation[0][2] * ray_x + rotation[1][2] * ray_y + rotation[2][2];
852 *source_x = focal_length * source_ray_x / source_ray_z + center_x;
853 *source_y = focal_length * source_ray_y / source_ray_z + center_y;
854 return isfinite(*source_x) && isfinite(*source_y);
870 double rotation[3][3],
double focal_length,
871 double center_x,
double center_y,
double ray_scale)
873 const double corners[4][2] = {
875 {(double)(source->
width - 1), 0.0},
876 {(double)(source->
width - 1), (double)(source->
height - 1)},
877 {0.0, (double)(source->
height - 1)}
879 for (
size_t index = 0; index < 4; index++) {
882 if (!
project_point(rotation, focal_length, center_x, center_y, ray_scale,
883 corners[index][0], corners[index][1], &source_x, &source_y)) {
903 double rotation[3][3],
double focal_length,
904 double center_x,
double center_y,
913 double best_log_scale = minimum_log_scale;
914 double best_zoom = INFINITY;
915 bool fit_found =
false;
918 double log_scale = minimum_log_scale * (1.0 - fraction);
919 double ray_scale = exp(log_scale);
922 center_x, center_y,
ray_scale, &candidate) != 0) {
926 if (zoom < best_zoom) {
928 best_log_scale = log_scale;
939 double lower_log_scale = fmax(minimum_log_scale, best_log_scale - coarse_step);
940 double upper_log_scale = fmin(0.0, best_log_scale + coarse_step);
942 double first_log_scale = (2.0 * lower_log_scale + upper_log_scale) / 3.0;
943 double second_log_scale = (lower_log_scale + 2.0 * upper_log_scale) / 3.0;
947 center_x, center_y, exp(first_log_scale),
950 center_x, center_y, exp(second_log_scale),
952 double first_zoom = first_valid ? 1.0 / (first_fit.
ray_scale * first_fit.
image_scale) : INFINITY;
953 double second_zoom = second_valid ? 1.0 / (second_fit.
ray_scale * second_fit.
image_scale) : INFINITY;
954 if (first_zoom < best_zoom) {
955 best_zoom = first_zoom;
958 if (second_zoom < best_zoom) {
959 best_zoom = second_zoom;
962 if (first_zoom <= second_zoom) {
963 upper_log_scale = second_log_scale;
965 lower_log_scale = first_log_scale;
986 double rotation[3][3],
double focal_length,
987 double center_x,
double center_y,
double ray_scale,
996 const double corners[4][2] = {
998 {(double)(source->
width - 1), 0.0},
999 {(double)(source->
width - 1), (double)(source->
height - 1)},
1000 {0.0, (double)(source->
height - 1)}
1002 double minimum_x = INFINITY;
1003 double minimum_y = INFINITY;
1004 double maximum_x = -INFINITY;
1005 double maximum_y = -INFINITY;
1006 for (
size_t index = 0; index < 4; index++) {
1010 corners[index][0], corners[index][1], &source_x, &source_y)) {
1014 minimum_x = fmin(minimum_x, source_x);
1015 minimum_y = fmin(minimum_y, source_y);
1016 maximum_x = fmax(maximum_x, source_x);
1017 maximum_y = fmax(maximum_y, source_y);
1020 double span_x = maximum_x - minimum_x;
1021 double span_y = maximum_y - minimum_y;
1027 double horizontal_fit = (double)(source->
width - 1) / span_x;
1028 double vertical_fit = (double)(source->
height - 1) / span_y;
1029 bool already_covered = minimum_x >= 0.0 && minimum_y >= 0.0
1030 && maximum_x <= (double)(source->
width - 1)
1031 && maximum_y <= (double)(source->
height - 1);
1032 if (already_covered && fit->
ray_scale == 1.0) {
1057 size_t left = (size_t)floor(source_x);
1058 size_t top = (size_t)floor(source_y);
1059 size_t right = left + 1 <
image->width ? left + 1 : left;
1060 size_t bottom = top + 1 <
image->height ? top + 1 : top;
1061 double horizontal = source_x - (double)left;
1062 double vertical = source_y - (double)top;
1068 for (
size_t component = 0; component <
JPEG_COMPONENTS; component++) {
1069 double top_value =
image->pixels[top_left + component] * (1.0 - horizontal)
1070 +
image->pixels[top_right + component] * horizontal;
1071 double bottom_value =
image->pixels[bottom_left + component] * (1.0 - horizontal)
1072 +
image->pixels[bottom_right + component] * horizontal;
1073 double value = top_value * (1.0 - vertical) + bottom_value * vertical;
1074 output[component] = (
unsigned char)(value + 0.5);
1088 struct stat original_status = {.st_mode = 0644};
1089 if (stat(filename, &original_status) != 0 && errno != ENOENT) {
1093 size_t filename_length = strlen(filename);
1094 const char suffix[] =
".transform.XXXXXX";
1096 if (filename_length >=
sizeof(temporary_name)
1097 ||
sizeof(suffix) >
sizeof(temporary_name) - filename_length) {
1098 errno = ENAMETOOLONG;
1101 int name_length = snprintf(temporary_name,
sizeof(temporary_name),
"%s%s", filename, suffix);
1102 if (name_length < 0 || (
size_t)name_length >=
sizeof(temporary_name)) {
1103 errno = ENAMETOOLONG;
1107 int temporary_fd = mkstemp(temporary_name);
1108 if (temporary_fd < 0) {
1111 FILE *
output = fdopen(temporary_fd,
"wb");
1113 close(temporary_fd);
1114 unlink(temporary_name);
1122 if (
result == 0 && fsync(temporary_fd) != 0) {
1125 if (
result == 0 && fchmod(temporary_fd, original_status.st_mode & 0777) != 0) {
1128 if (fclose(
output) != 0) {
1131 if (
result == 0 && rename(temporary_name, filename) != 0) {
1135 unlink(temporary_name);
1136 fprintf(stderr,
"MOCK_TRANSFORM:\tfailed to replace %s: %s\n", filename, strerror(errno));
void(* close)(struct usbfs_handle *)
#define POSITION_BFP_SCALE
Reject invalid Huffman selectors before the bundled codec indexes its tables.
static void usher_jpeg_validate_marker(j_common_ptr state)
Validate marker parameters from an error manager's emit_message callback.
struct jpeg_compress_struct encoder
uint8_t row[LWIR_MAX_WIDTH *3U]
struct jpeg_error_mgr error
struct jpeg_decompress_struct decoder
struct jpeg_compress_struct encoder
struct transform_jpeg_error encode_error
unsigned char destination[IMAGE_MOCK_RGB_CAPACITY]
struct transform_jpeg_error decode_error
struct jpeg_decompress_struct decoder
unsigned char source[IMAGE_MOCK_RGB_CAPACITY]
unsigned char mosaic[MAP_MOSAIC_CAPACITY]
struct usher_control_result result
struct captured_image image