Commit b3036d0c authored by Andrey Filippov's avatar Andrey Filippov
Browse files

working on Jacobian

parent c14405c9

src/main/java/com/elphel/imagej/tileprocessor/Corr2dLMA.java

+58 −10
Original line number Original line Diff line number Diff line
@@ -132,6 +132,7 @@ public class Corr2dLMA { 
    private final double [][] corr_wnd;

    private final double [][] corr_wnd;

    private boolean [] used_cameras;

    private boolean [] used_cameras;

    private final Matrix [] m_disp = new Matrix[NUM_CAMS];

    private final Matrix [] m_disp = new Matrix[NUM_CAMS];

    private int ncam = 0; // number of used cameras

    private int     last_cam; // index of the last camera (special treatment for disparity correction)

    private int     last_cam; // index of the last camera (special treatment for disparity correction)

    private boolean second_last; // there is a pair where the second camera is the last one (false: first in a pair is the last one)

    private boolean second_last; // there is a pair where the second camera is the last one (false: first in a pair is the last one)

    private final Matrix [][] m_pairs =      new Matrix[NUM_CAMS][NUM_CAMS];

    private final Matrix [][] m_pairs =      new Matrix[NUM_CAMS][NUM_CAMS];
@@ -294,7 +295,7 @@ public class Corr2dLMA { 
			used_pairs[pindx[s.fcam][s.scam]]=true; // throws < 0 - wrong pair, f==s

			used_pairs[pindx[s.fcam][s.scam]]=true; // throws < 0 - wrong pair, f==s

			used_pairs_dir[s.fcam][s.scam] = true;

			used_pairs_dir[s.fcam][s.scam] = true;

		}

		}

		int ncam = 0, npairs=0;

		int npairs=0;

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

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

			USED_CAMS_MAP[i] = ncam;

			USED_CAMS_MAP[i] = ncam;

			if (used_cameras[i]) {

			if (used_cameras[i]) {
@@ -425,7 +426,7 @@ public class Corr2dLMA { 
		Matrix [] xcam_ycam = new Matrix[NUM_CAMS];

		Matrix [] xcam_ycam = new Matrix[NUM_CAMS];

		for (int i = 0; i < NUM_CAMS; i++) if (used_cameras[i]) {

		for (int i = 0; i < NUM_CAMS; i++) if (used_cameras[i]) {

			double [] add_dnd = {av[DISP_INDEX]+ av[DDISP_INDEX + i],  av[NDISP_INDEX + i]};

			double [] add_dnd = {av[DISP_INDEX]+ av[DDISP_INDEX + i],  av[NDISP_INDEX + i]};

			xcam_ycam[i] = m_disp[i].arrayTimes(new Matrix(add_dnd,2));

			xcam_ycam[i] = m_disp[i].times(new Matrix(add_dnd,2));

		}

		}

		double [][][] xp_yp = new double[NUM_CAMS][NUM_CAMS][];

		double [][][] xp_yp = new double[NUM_CAMS][NUM_CAMS][];

		double [] axc_yc = {transform_size - 1.0, transform_size-1.0};

		double [] axc_yc = {transform_size - 1.0, transform_size-1.0};
@@ -461,16 +462,63 @@ public class Corr2dLMA { 
			fx[ns] = d * Gp;

			fx[ns] = d * Gp;

			int np = 0;

			int np = 0;

			if (jt != null) {

			if (jt != null) {

				if (par_mask[DISP_INDEX])  jt[np++][ns] = d;

				if (par_mask[DISP_INDEX])      jt[np++][ns] = 2 * WGp *

						((A * xmxp + B * ymyp) * m_pairs[s.fcam][s.scam].get(0, 0)+

						 (B * xmxp + C * ymyp) * m_pairs[s.fcam][s.scam].get(1, 0));

				if (par_mask[A_INDEX])         jt[np++][ns] = -WGp*(xmxp2 + ymyp2);

				if (par_mask[A_INDEX])         jt[np++][ns] = -WGp*(xmxp2 + ymyp2);

				if (par_mask[B_INDEX])         jt[np++][ns] = -WGp* 2 * xmxp_ymyp;

				if (par_mask[B_INDEX])         jt[np++][ns] = -WGp* 2 * xmxp_ymyp;

				if (par_mask[CMA_INDEX])       jt[np++][ns] = -WGp* ymyp2;

				if (par_mask[CMA_INDEX])       jt[np++][ns] = -WGp* ymyp2;

				if (par_mask[G0_INDEX + pair]) jt[np++][ns] = d;

// USED_PAIRS_MAP

// USED_CAMS_MAP

				// process ddisp (last camera not used, is equal to minus sum of others to make a sum == 0)

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

					jt[np + USED_CAMS_MAP[f]][ns] = 0.0;

				}

				if (par_mask[DDISP_INDEX + s.fcam] && (par_mask[DDISP_INDEX + s.scam] )) {

					if (s.fcam != last_cam) {

						 jt[np + USED_CAMS_MAP[s.fcam]][ns] += 2 * WGp *

									((A * xmxp + B * ymyp) * m_disp[s.fcam].get(0, 0)+

									 (B * xmxp + C * ymyp) * m_disp[s.fcam].get(1, 0));





					} else { // last camera - use all others with minus sign

						for (int c = 0; c < NUM_CAMS; c++) if ((c != last_cam) && par_mask[DDISP_INDEX + c]) {

							 jt[np + USED_CAMS_MAP[c]][ns] -= 2 * WGp *

										((A * xmxp + B * ymyp) * m_disp[c].get(0, 0)+

										 (B * xmxp + C * ymyp) * m_disp[c].get(1, 0));

						}





					}

					if (s.scam != last_cam) {

						 jt[np + USED_CAMS_MAP[s.scam]][ns] -= 2 * WGp *

									((A * xmxp + B * ymyp) * m_disp[s.scam].get(0, 0)+

									 (B * xmxp + C * ymyp) * m_disp[s.scam].get(1, 0));





					} else {

						for (int c = 0; c < NUM_CAMS; c++) if ((c != last_cam) && par_mask[DDISP_INDEX + c]) {

							 jt[np + USED_CAMS_MAP[c]][ns] += 2 * WGp *

										((A * xmxp + B * ymyp) * m_disp[c].get(0, 0)+

										 (B * xmxp + C * ymyp) * m_disp[c].get(1, 0));

						}





					}

					}

				}

				}

				np += ncam;

				// process ndisp

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

					jt[np + USED_CAMS_MAP[f]][ns] = 0.0;

				}

				if (par_mask[NDISP_INDEX + s.fcam] && (par_mask[NDISP_INDEX + s.scam] )) {

						 jt[np + USED_CAMS_MAP[s.fcam]][ns] += 2 * WGp *

									((A * xmxp + B * ymyp) * m_disp[s.fcam].get(0, 1)+

									 (B * xmxp + C * ymyp) * m_disp[s.fcam].get(1, 1));

						 jt[np + USED_CAMS_MAP[s.scam]][ns] -= 2 * WGp *

									((A * xmxp + B * ymyp) * m_disp[s.scam].get(0, 1)+

									 (B * xmxp + C * ymyp) * m_disp[s.scam].get(1, 1));

				}

				np += ncam;

			}

		}

		// add 2 extra samples - damping cost_wy, cost_wxy

		// add 2 extra samples - damping cost_wy, cost_wxy

		fx[num_samples] =     av[WM_INDEX];

		fx[num_samples] =     av[WM_INDEX];

		fx[num_samples + 1] = av[WXY_INDEX];

		fx[num_samples + 1] = av[WXY_INDEX];