geometry_correction.h 7.7 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43
/**
 **
 ** geometry_correction.h
 **
 ** Copyright (C) 2020 Elphel, Inc.
 **
 ** -----------------------------------------------------------------------------**
 **
 **  geometry_correction.h is free software: you can redistribute it and/or modify
 **  it under the terms of the GNU General Public License as published by
 **  the Free Software Foundation, either version 3 of the License, or
 **  (at your option) any later version.
 **
 **  This program is distributed in the hope that it will be useful,
 **  but WITHOUT ANY WARRANTY; without even the implied warranty of
 **  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 **  GNU General Public License for more details.
 **
 **  You should have received a copy of the GNU General Public License
 **  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 **
 **  Additional permission under GNU GPL version 3 section 7
 **
 **  If you modify this Program, or any covered work, by linking or
 **  combining it with NVIDIA Corporation's CUDA libraries from the
 **  NVIDIA CUDA Toolkit (or a modified version of those libraries),
 **  containing parts covered by the terms of NVIDIA CUDA Toolkit
 **  EULA, the licensors of this Program grant you additional
 **  permission to convey the resulting work.
 ** -----------------------------------------------------------------------------**
 */

/**
**************************************************************************
* \file geometry_correction.h
* \brief header file for geometry correction - per-tile/per camera calculation of the tile offset

*/
#pragma once
#ifndef NUM_CAMS
#include "tp_defines.h"
#endif

44

Andrey Filippov's avatar
Andrey Filippov committed
45 46 47 48 49 50 51 52 53 54 55 56 57
#define NVRTC_BUG 1
#ifndef M_PI
#define M_PI  3.14159265358979323846 /* pi */
#endif
#ifndef offsetof
#define offsetof(st, m) \
    ((size_t)&(((st *)0)->m))
//#define offsetof(TYPE, MEMBER) __builtin_offsetof (TYPE, MEMBER)
#endif


#define SCENE_UNITS_SCALE  0.001 // meters from mm
#define MIN_DISPARITY      0.01  // minimal disparity to try to convert to world coordinates
58 59 60 61 62 63
struct tp_task {
	int   task;
	union {
		int      txy;
		unsigned short sxy[2];
	};
64
	float target_disparity;
65 66
	float centerXY[2];          // "ideal" centerX, centerY to use instead of the uniform tile centers (txy) for interscene accumulation
	                            // if isnan(centerXY[0]), then txy is used to calculate centerXY and all xy
67 68 69
	// scale == 0 - old way, just set. Scale !=0 - accumulate. Or make > 0 - set too? only negative - subtract?
	float scale;                // multiply during direct conversion before accumulating in TD - used for motion blur correction
	float xy       [NUM_CAMS][2];
70
	float disp_dist[NUM_CAMS][4]; // calculated with getPortsCoordinates()
71 72
};

73
#define get_task_size(x) (sizeof(struct tp_task)/sizeof(float) - 6 * (NUM_CAMS - x))
74 75 76 77 78 79
#define TP_TASK_TASK_OFFSET      0
#define TP_TASK_TXY_OFFSET       1
#define TP_TASK_DISPARITY_OFFSET 2
#define TP_TASK_CENTERXY_OFFSET  3
#define TP_TASK_SCALE_OFFSET     5
#define TP_TASK_XY_OFFSET        6
80

81 82 83 84 85 86 87 88 89
struct corr_vector{
	float tilt    [NUM_CAMS-1]; // 0..2
	float azimuth [NUM_CAMS-1]; // 3..5
	float roll    [NUM_CAMS];   // 6..9
	float zoom    [NUM_CAMS-1]; // 10..12
	// for ERS correction:
	float imu_rot [3]; // d_tilt/dt (rad/s), d_az/dt, d_roll/dt 13..15
	float imu_move[3]; // dx/dt, dy/dt, dz/dt 16..19
};
Andrey Filippov's avatar
Andrey Filippov committed
90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109
#ifdef NVRTC_BUG
struct trot_deriv{
	float rots    [NUM_CAMS][3][3];
	float d_daz   [NUM_CAMS][3][3];
	float d_tilt  [NUM_CAMS][3][3];
	float d_roll  [NUM_CAMS][3][3];
	float d_zoom  [NUM_CAMS][3][3];
};
#else
union trot_deriv{
	struct {
		float rots    [NUM_CAMS][3][3];
		float d_daz   [NUM_CAMS][3][3];
		float d_tilt  [NUM_CAMS][3][3];
		float d_roll  [NUM_CAMS][3][3];
		float d_zoom  [NUM_CAMS][3][3];
	};
	float matrices [5][NUM_CAMS][3][3];
};
#endif
110 111

struct gc {
Andrey Filippov's avatar
Andrey Filippov committed
112 113 114
	float pixelCorrectionWidth; //  =2592;   // virtual camera center is at (pixelCorrectionWidth/2, pixelCorrectionHeight/2)
	float pixelCorrectionHeight; // =1936;
	float line_time;        // duration of one scan line readout (for ERS)
115 116 117
	float focalLength;      // =FOCAL_LENGTH;
	float pixelSize;        // =  PIXEL_SIZE; //um
	float distortionRadius; // =  DISTORTION_RADIUS; // mm - half width of the sensor
Andrey Filippov's avatar
Andrey Filippov committed
118 119 120 121 122 123 124 125 126 127 128 129
#ifndef	NVRTC_BUG
	union {
		struct {
#endif
			float distortionC;      // r^2
			float distortionB;      // r^3
			float distortionA;      // r^4 (normalized to focal length or to sensor half width?)
			float distortionA5;     //r^5 (normalized to focal length or to sensor half width?)
			float distortionA6;     //r^6 (normalized to focal length or to sensor half width?)
			float distortionA7;     //r^7 (normalized to focal length or to sensor half width?)
			float distortionA8;     //r^8 (normalized to focal length or to sensor half width?)
#ifndef	NVRTC_BUG
Andrey Filippov's avatar
Andrey Filippov committed
130 131 132
		};
		float rad_coeff [7];
	};
Andrey Filippov's avatar
Andrey Filippov committed
133
#endif
134 135 136 137 138 139 140
	// parameters, common for all sensors
	float    elevation;     // degrees, up - positive;
	float    heading;       // degrees, CW (from top) - positive

	float forward    [NUM_CAMS];
	float right      [NUM_CAMS];
	float height     [NUM_CAMS];
Andrey Filippov's avatar
Andrey Filippov committed
141 142
	float roll       [NUM_CAMS];    // degrees, CW (to target) - positive
	float pXY0       [NUM_CAMS][2];
143 144 145 146 147 148
	float common_right;    // mm right, camera center
	float common_forward;  // mm forward (to target), camera center
	float common_height;   // mm up, camera center
	float common_roll;     // degrees CW (to target) camera as a whole
//	float [][] XYZ_he;     // all cameras coordinates transformed to eliminate heading and elevation (rolls preserved)
//	float [][] XYZ_her = null; // XYZ of the lenses in a corrected CCS (adjusted for to elevation, heading,  common_roll)
Andrey Filippov's avatar
Andrey Filippov committed
149
	float rXY        [NUM_CAMS][2]; // XY pairs of the in a normal plane, relative to disparityRadius
150 151 152 153
//	float [][] rXY_ideal = {{-0.5, -0.5}, {0.5,-0.5}, {-0.5, 0.5}, {0.5,0.5}};
// only used for the multi-quad systems
	float cameraRadius; // =0; // average distance from the "mass center" of the sensors to the sensors
	float disparityRadius; // =150.0; // distance between cameras to normalize disparity units to. sqrt(2)*disparityRadius for quad
154
	float woi_tops   [NUM_CAMS]; // used to calculate scanline timing
155
};
Andrey Filippov's avatar
Andrey Filippov committed
156 157
#define RAD_COEFF_LEN 7
extern "C" __global__ void get_tiles_offsets(
158 159 160 161
		int                  uniform_grid, //==0: use provided centers (as for interscene) , !=0 calculate uniform grid
		int                  num_cams,
//		struct tp_task     * gpu_tasks,
		float              * gpu_ftasks,         // flattened tasks, 27 floats for quad EO, 99 floats for LWIR16
Andrey Filippov's avatar
Andrey Filippov committed
162 163 164 165 166 167
		int                  num_tiles,          // number of tiles in task
		struct gc          * gpu_geometry_correction,
		struct corr_vector * gpu_correction_vector,
		float *              gpu_rByRDist, // length should match RBYRDIST_LEN
		trot_deriv   * gpu_rot_deriv);

Andrey Filippov's avatar
Andrey Filippov committed
168
extern "C" __global__ void calculate_tiles_offsets(
169 170 171 172
		int                  uniform_grid, //==0: use provided centers (as for interscene) , !=0 calculate uniform grid
		int                  num_cams,
		float              * gpu_ftasks,         // flattened tasks, 27 floats for quad EO, 99 floats for LWIR16
//		struct tp_task     * gpu_tasks,
Andrey Filippov's avatar
Andrey Filippov committed
173 174 175 176 177 178 179
		int                  num_tiles,          // number of tiles in task
		struct gc          * gpu_geometry_correction,
		struct corr_vector * gpu_correction_vector,
		float *              gpu_rByRDist, // length should match RBYRDIST_LEN
		trot_deriv   * gpu_rot_deriv);


Andrey Filippov's avatar
Andrey Filippov committed
180 181
// uses NUM_CAMS blocks, (3,3,3) threads
extern "C" __global__ void calc_rot_deriv(
182
		int                  num_cams,
Andrey Filippov's avatar
Andrey Filippov committed
183 184
		struct corr_vector * gpu_correction_vector,
		trot_deriv   * gpu_rot_deriv);
185

186 187 188 189 190 191
#define CALC_REVERSE_TABLE_BLOCK_THREADS (NUM_CAMS * 3 * 3 * 3) // fixed blockDim
// Use same blocks/threads as with calc_rot_deriv() - NUM_CAMS blocks, (3,3,3) threads
extern "C" __global__ void calcReverseDistortionTable(
		struct gc * geometry_correction,
		float * rByRDist);

192

193