Commit 705a2e85 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

updated ers correction

parent 263efc60

src/geometry_correction.cu

+44 −42
Original line number Diff line number Diff line
@@ -294,7 +294,6 @@ extern "C" __global__ void get_tiles_offsets( 
		float *              gpu_rByRDist,      // length should match RBYRDIST_LEN

		trot_deriv   * gpu_rot_deriv)

{

//	int task_num = blockIdx.x * blockDim.x + threadIdx.x; //  blockIdx.x * TILES_PER_BLOCK_GEOM + threadIdx.x

	int task_num = blockIdx.x * blockDim.y + threadIdx.y; //  blockIdx.x * TILES_PER_BLOCK_GEOM + threadIdx.y

	if (task_num >= num_tiles){

		return;
@@ -306,6 +305,7 @@ extern "C" __global__ void get_tiles_offsets( 
	__shared__ float rByRDist [RBYRDIST_LEN];

	__shared__ struct corr_vector extrinsic_corr;

	__shared__ trot_deriv rot_deriv;

	__shared__ float pY_offsets[TILES_PER_BLOCK_GEOM][NUM_CAMS];

	float pXY[2]; // result to be copied to task

	// copy data common to all threads

	{
@@ -362,8 +362,7 @@ extern "C" __global__ void get_tiles_offsets( 
			(extrinsic_corr.imu_move[0] != 0.0) ||

			(extrinsic_corr.imu_move[1] != 0.0) ||

			(extrinsic_corr.imu_move[2] != 0.0);

// Temporary

//	imu_exists = 0;



#ifdef DEBUG21

	if ((ncam == DBG_CAM)  && (task_num == DBG_TILE)){

		printf("\nTile = %d, camera= %d\n", task_num, ncam);
@@ -373,6 +372,9 @@ extern "C" __global__ void get_tiles_offsets( 
	}

	__syncthreads();// __syncwarp();

#endif // DEBUG21







	//		String dbg_s = corr_vector.toString();

	/* Starting with required tile center X, Y and nominal distortion, for each sensor port:

	 * 1) unapply common distortion (maybe for different - master camera)
@@ -401,15 +403,10 @@ extern "C" __global__ void get_tiles_offsets( 
	float pXcd = px - 0.5 * geometry_correction.pixelCorrectionWidth;

	float pYcd = py - 0.5 * geometry_correction.pixelCorrectionHeight;



//	float rXY [NUM_CAMS][2];

	float rXY [2];



//	for (int i = 0; i < NUM_CAMS;i++){

//	rXY[ncam][0] = geometry_correction.rXY[ncam][0];

//	rXY[ncam][1] = geometry_correction.rXY[ncam][1];

	rXY[0] = geometry_correction.rXY[ncam][0];

	rXY[1] = geometry_correction.rXY[ncam][1];

//	}



	float rD = sqrtf(pXcd*pXcd + pYcd*pYcd)*0.001*geometry_correction.pixelSize; // distorted radius in a virtual center camera

	float rND2R=getRByRDist(rD/geometry_correction.distortionRadius, rByRDist);
@@ -489,9 +486,17 @@ extern "C" __global__ void get_tiles_offsets( 
	float pYid = pYci * rD2rND;

	pXY[0] =  pXid + geometry_correction.pXY0[ncam][0];

	pXY[1] =  pYid + geometry_correction.pXY0[ncam][1];



// new for ERS

	pY_offsets[threadIdx.y][ncam] = pXY[1] - geometry_correction.woi_tops[ncam];

	__syncthreads();

	// Each thread re-calculate same sum

	float lines_avg = 0;

	for (int i = 0; i < NUM_CAMS; i ++){

		lines_avg += pY_offsets[threadIdx.y][i];

	}

	lines_avg *= (1.0/NUM_CAMS);

	// used when calculating derivatives, TODO: combine calculations !



	float pY_offset = pY_offsets[threadIdx.y][ncam] - lines_avg;

#ifdef DEBUG21

	if ((ncam == DBG_CAM)  && (task_num == DBG_TILE)){

		printf("pXci0 = %f,  pYci0 = %f\n", pXci0, pYci0);
@@ -501,6 +506,7 @@ extern "C" __global__ void get_tiles_offsets( 
		printf("rD2rND = %f\n", rD2rND);

		printf("pXid = %f,  pYid = %f\n", pXid, pYid);

		printf("pXY[0] = %f,  pXY[1] = %f\n", pXY[0], pXY[1]); // OK

		printf("lines_avg = %f,  pY_offset = %f\n", lines_avg, pY_offset);

	}

	__syncthreads();// __syncwarp();

#endif // DEBUG21
@@ -514,14 +520,10 @@ extern "C" __global__ void get_tiles_offsets( 


#pragma unroll

	for (int j = 0; j< 3; j++){

//		drvi_daz[j] = rot_deriv.d_daz[ncam][j][0] *  rvi[0] + rot_deriv.d_daz[ncam][j][1] *  rvi[1] + rot_deriv.d_daz[ncam][j][2] *  rvi[2];

//		drvi_dtl[j] = rot_deriv.d_tilt[ncam][j][0] * rvi[0] + rot_deriv.d_tilt[ncam][j][1] * rvi[1] + rot_deriv.d_tilt[ncam][j][2] * rvi[2];

//		drvi_drl[j] = rot_deriv.d_roll[ncam][j][0] * rvi[0] + rot_deriv.d_roll[ncam][j][1] * rvi[1] + rot_deriv.d_roll[ncam][j][2] * rvi[2];

		drvi_daz[j] = rot_deriv.d_daz[ncam][j][0] *  pXci0 + rot_deriv.d_daz[ncam][j][1] *  pYci0 + rot_deriv.d_daz[ncam][j][2] *  fl_pix;

		drvi_dtl[j] = rot_deriv.d_tilt[ncam][j][0] * pXci0 + rot_deriv.d_tilt[ncam][j][1] * pYci0 + rot_deriv.d_tilt[ncam][j][2] * fl_pix;

		drvi_drl[j] = rot_deriv.d_roll[ncam][j][0] * pXci0 + rot_deriv.d_roll[ncam][j][1] * pYci0 + rot_deriv.d_roll[ncam][j][2] * fl_pix;

	}

//			double [][] avi = {{pXci0}, {pYci0},{fl_pix}};



	float dpXci_dazimuth = drvi_daz[0] * norm_z - pXci * drvi_daz[2] / rvi[2];

	float dpYci_dazimuth = drvi_daz[1] * norm_z - pYci * drvi_daz[2] / rvi[2];
@@ -573,25 +575,6 @@ extern "C" __global__ void get_tiles_offsets( 
	// unity vector in the direction of radius

	float c_dist = pXci/rNDi;

	float s_dist = pYci/rNDi;

/*

				double [][] arot2= {

						{c_dist, s_dist},

						{-s_dist, c_dist}};

				Matrix rot2 = new Matrix(arot2); // convert from non-distorted X,Y to parallel and perpendicular (CCW) to the radius



				double [][] ascale_distort = {

						{rD2rND + ri* drD2rND_dri, 0     },

						{0,                       rD2rND}};

				Matrix scale_distort = new Matrix(ascale_distort); // scale component parallel to radius as distortion derivative, perpendicular - as distortion



				Matrix dd2 = rot2.transpose().times(scale_distort).times(rot2).times(dd1);



				disp_dist[i][0] =   dd2.get(0, 0);

				disp_dist[i][1] =   dd2.get(0, 1);

				disp_dist[i][2] =   dd2.get(1, 0); // d_py/d_disp

				disp_dist[i][3] =   dd2.get(1, 1);



 */

//#undef NVRTC_BUG

	float drD2rND_dri = 0.0;

	{
@@ -612,11 +595,6 @@ extern "C" __global__ void get_tiles_offsets( 
	}

	float scale_distort00 = rD2rND + ri* drD2rND_dri;

	float scale_distort11 = rD2rND;

//	float rot2Xdd1[2][2];

//	rot2Xdd1[0][0] =  c_dist * dd1[0][0] + s_dist * dd1[1][0];

//	rot2Xdd1[0][1] =  c_dist * dd1[0][1] + s_dist * dd1[1][1];

//	rot2Xdd1[1][0] = -s_dist * dd1[0][0] + c_dist * dd1[1][0];

//	rot2Xdd1[1][1] = -s_dist * dd1[0][1] + c_dist * dd1[1][1];

	float scale_distortXrot2Xdd1[2][2];

	scale_distortXrot2Xdd1[0][0] = ( c_dist * dd1[0][0] + s_dist * dd1[1][0]) * scale_distort00;

	scale_distortXrot2Xdd1[0][1] = ( c_dist * dd1[0][1] + s_dist * dd1[1][1]) * scale_distort00;
@@ -651,6 +629,7 @@ extern "C" __global__ void get_tiles_offsets( 
//	float imu_move[3]; // dx/dt, dy/dt, dz/dt 16..19 geometry_correction.imu_move

// ERS linear does not yet use per-port rotations, probably not needed

	if (imu_exists){

		/*

		float delta_t = disp_dist[2] * disparity * geometry_correction.line_time; // positive for top cameras, negative - for bottom //disp_dist[2]=dd2.get(1, 0)

		float ers_Xci =	delta_t * (

				dpXci_dtilt * extrinsic_corr.imu_rot[0] +
@@ -660,9 +639,22 @@ extern "C" __global__ void get_tiles_offsets( 
				dpYci_dtilt * extrinsic_corr.imu_rot[0] +

				dpYci_dazimuth * extrinsic_corr.imu_rot[1] +

				dpYci_droll * extrinsic_corr.imu_rot[2]);

		 */

		float ers_x =

				dpXci_dtilt * extrinsic_corr.imu_rot[0] +

				dpXci_dazimuth * extrinsic_corr.imu_rot[1]  +

				dpXci_droll * extrinsic_corr.imu_rot[2];

		float ers_y =

				dpYci_dtilt * extrinsic_corr.imu_rot[0] +

				dpYci_dazimuth * extrinsic_corr.imu_rot[1] +

				dpYci_droll * extrinsic_corr.imu_rot[2];







#ifdef DEBUG21

		if ((ncam == DBG_CAM)  && (task_num == DBG_TILE)){

			printf("delta_t = %f,  ers_Xci = %f,  ers_Yci = %f\n", delta_t, ers_Xci, ers_Yci);

//			printf("delta_t = %f,  ers_Xci = %f,  ers_Yci = %f\n", delta_t, ers_Xci, ers_Yci);

			printf("ers_x = %f,  ers_y = %f\n", ers_x, ers_y);

		}

		__syncthreads();// __syncwarp();

#endif // DEBUG21
@@ -674,14 +666,22 @@ extern "C" __global__ void get_tiles_offsets( 
			dpXci_pYci_imu_lin[1][1] =  wdisparity / k; // dpy/ dworld_Y

			dpXci_pYci_imu_lin[0][2] =  (xyz[0] / k) * dwdisp_dz; // dpx/ dworld_Z

			dpXci_pYci_imu_lin[1][2] =  (xyz[1] / k) * dwdisp_dz; // dpy/ dworld_Z

			/*

			ers_Xci += delta_t* (

					dpXci_pYci_imu_lin[0][0] * extrinsic_corr.imu_move[0] +

					dpXci_pYci_imu_lin[0][2] * extrinsic_corr.imu_move[2]);

			ers_Yci += delta_t* (

					dpXci_pYci_imu_lin[1][1] * extrinsic_corr.imu_move[1] +

					dpXci_pYci_imu_lin[1][2] * extrinsic_corr.imu_move[2]);

			pXY[0] +=  ers_Xci * rD2rND; // added correction to pixel X

			pXY[1] +=  ers_Yci * rD2rND; // added correction to pixel Y

			*/

			ers_x += dpXci_pYci_imu_lin[0][0] * extrinsic_corr.imu_move[0] +

					 dpXci_pYci_imu_lin[0][2] * extrinsic_corr.imu_move[2];

			ers_y += dpXci_pYci_imu_lin[1][1] * extrinsic_corr.imu_move[1] +

					 dpXci_pYci_imu_lin[1][2] * extrinsic_corr.imu_move[2];

			float delta_t = (pY_offset/ (1.0 - geometry_correction.line_time * ers_y)) * geometry_correction.line_time; // positive for top cameras, negative - for bottom //disp_dist[2]=dd2.get(1, 0)



			pXY[0] +=   delta_t * ers_x * rD2rND; // added correction to pixel X

			pXY[1] +=   delta_t * ers_y * rD2rND; // added correction to pixel Y



#ifdef DEBUG21

			if ((ncam == DBG_CAM)  && (task_num == DBG_TILE)){
@@ -689,7 +689,7 @@ extern "C" __global__ void get_tiles_offsets( 
				printf("dpXci_pYci_imu_lin[0][0] = %f,  dpXci_pYci_imu_lin[0][2] = %f\n", dpXci_pYci_imu_lin[0][0],dpXci_pYci_imu_lin[0][2]);

				printf("dpXci_pYci_imu_lin[1][1] = %f,  dpXci_pYci_imu_lin[1][2] = %f\n", dpXci_pYci_imu_lin[1][1],dpXci_pYci_imu_lin[1][2]);



				printf("delta_t = %f,  ers_Xci = %f,  ers_Yci = %f\n", delta_t, ers_Xci, ers_Yci);

				printf("delta_t = %f,  ers_x = %f,  ers_y = %f\n", delta_t, ers_x, ers_y);

				printf("pXY[0] = %f,  pXY[1] = %f\n", pXY[0], pXY[1]); // OK

			}

			__syncthreads();// __syncwarp();
@@ -703,6 +703,7 @@ extern "C" __global__ void get_tiles_offsets( 




}



extern "C" __global__ void calcReverseDistortionTable(

		struct gc * geometry_correction,

		float * rByRDist)
@@ -841,6 +842,7 @@ __device__ void printGeometryCorrection(struct gc * g){ 


	printf("%22s: %f\n","cameraRadius",    g->cameraRadius);

	printf("%22s: %f\n","disparityRadius", g->disparityRadius);

	printf("%22s: %f, %f, %f, %f \n","woi_tops", g->woi_tops[0], g->woi_tops[1], g->woi_tops[2], g->woi_tops[3]);

#endif //ifndef JCUDA

}

src/geometry_correction.h

+1 −0
Original line number Diff line number Diff line
@@ -138,6 +138,7 @@ struct gc { 
// 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

	float woi_tops   [NUM_CAMS]; // used to calculate scanline timing

};

#define RAD_COEFF_LEN 7

extern "C" __global__ void get_tiles_offsets(

src/test_tp.cu

+34 −34
Original line number Diff line number Diff line
@@ -251,50 +251,50 @@ int main(int argc, char **argv) 
    // CLT testing



    const char* kernel_file[] = {

    		"/data_ssd/git/tile_processor_gpu/clt/main_chn0_transposed.kernel",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn1_transposed.kernel",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn2_transposed.kernel",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn3_transposed.kernel"};

    		"/home/eyesis/git/tile_processor_gpu/clt/main_chn0_transposed.kernel",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn1_transposed.kernel",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn2_transposed.kernel",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn3_transposed.kernel"};



    const char* kernel_offs_file[] = {

    		"/data_ssd/git/tile_processor_gpu/clt/main_chn0_transposed.kernel_offsets",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn1_transposed.kernel_offsets",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn2_transposed.kernel_offsets",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn3_transposed.kernel_offsets"};

    		"/home/eyesis/git/tile_processor_gpu/clt/main_chn0_transposed.kernel_offsets",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn1_transposed.kernel_offsets",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn2_transposed.kernel_offsets",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn3_transposed.kernel_offsets"};



    const char* image_files[] = {

    		"/data_ssd/git/tile_processor_gpu/clt/main_chn0.bayer",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn1.bayer",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn2.bayer",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn3.bayer"};

    		"/home/eyesis/git/tile_processor_gpu/clt/main_chn0.bayer",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn1.bayer",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn2.bayer",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn3.bayer"};



    const char* ports_offs_xy_file[] = {

    		"/data_ssd/git/tile_processor_gpu/clt/main_chn0.portsxy",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn1.portsxy",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn2.portsxy",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn3.portsxy"};

    		"/home/eyesis/git/tile_processor_gpu/clt/main_chn0.portsxy",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn1.portsxy",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn2.portsxy",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn3.portsxy"};



#ifndef DBG_TILE

    const char* ports_clt_file[] = { // never referenced

    		"/data_ssd/git/tile_processor_gpu/clt/main_chn0.clt",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn1.clt",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn2.clt",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn3.clt"};

    		"/home/eyesis/git/tile_processor_gpu/clt/main_chn0.clt",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn1.clt",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn2.clt",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn3.clt"};

    const char* result_rbg_file[] = {

    		"/data_ssd/git/tile_processor_gpu/clt/main_chn0.rbg",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn1.rbg",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn2.rbg",

			"/data_ssd/git/tile_processor_gpu/clt/main_chn3.rbg"};

    		"/home/eyesis/git/tile_processor_gpu/clt/main_chn0.rbg",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn1.rbg",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn2.rbg",

			"/home/eyesis/git/tile_processor_gpu/clt/main_chn3.rbg"};

#endif

    const char* result_corr_file = "/data_ssd/git/tile_processor_gpu/clt/main_corr.corr";

    const char* result_corr_quad_file =  "/data_ssd/git/tile_processor_gpu/clt/main_corr-quad.corr";

    const char* result_corr_cross_file = "/data_ssd/git/tile_processor_gpu/clt/main_corr-cross.corr";

    const char* result_textures_file =       "/data_ssd/git/tile_processor_gpu/clt/texture.rgba";

    const char* result_textures_rgba_file = "/data_ssd/git/tile_processor_gpu/clt/texture_rgba.rgba";

    const char* result_corr_file = "/home/eyesis/git/tile_processor_gpu/clt/main_corr.corr";

    const char* result_corr_quad_file =  "/home/eyesis/git/tile_processor_gpu/clt/main_corr-quad.corr";

    const char* result_corr_cross_file = "/home/eyesis/git/tile_processor_gpu/clt/main_corr-cross.corr";

    const char* result_textures_file =       "/home/eyesis/git/tile_processor_gpu/clt/texture.rgba";

    const char* result_textures_rgba_file = "/home/eyesis/git/tile_processor_gpu/clt/texture_rgba.rgba";



    const char* rByRDist_file =            "/data_ssd/git/tile_processor_gpu/clt/main.rbyrdist";

    const char* correction_vector_file =   "/data_ssd/git/tile_processor_gpu/clt/main.correction_vector";

    const char* geometry_correction_file = "/data_ssd/git/tile_processor_gpu/clt/main.geometry_correction";

    const char* rByRDist_file =            "/home/eyesis/git/tile_processor_gpu/clt/main.rbyrdist";

    const char* correction_vector_file =   "/home/eyesis/git/tile_processor_gpu/clt/main.correction_vector";

    const char* geometry_correction_file = "/home/eyesis/git/tile_processor_gpu/clt/main.geometry_correction";





    float port_offsets[NUM_CAMS][2] =  {// used only in textures to scale differences
@@ -1101,8 +1101,8 @@ int main(int argc, char **argv) 
			(corr_size_combo * corr_size_combo) * sizeof(float),

			num_corr_combo,

    		cudaMemcpyDeviceToHost));

//    const char* result_corr_quad_file =  "/data_ssd/git/tile_processor_gpu/clt/main_corr-quad.corr";

//    const char* result_corr_cross_file = "/data_ssd/git/tile_processor_gpu/clt/main_corr-cross.corr";

//    const char* result_corr_quad_file =  "/home/eyesis/git/tile_processor_gpu/clt/main_corr-quad.corr";

//    const char* result_corr_cross_file = "/home/eyesis/git/tile_processor_gpu/clt/main_corr-cross.corr";



#ifndef NSAVE_CORR

    		printf("Writing phase correlation data to %s\n",  result_corr_quad_file);