Refines the initial line estimates using EM.  The number of iterations is fixed.
763812
protected boolean optimize(Polygon2D_F64 seed , Polygon2D_F64 current ) {

		previous.set(seed);

		// pixels squares is faster to compute
		double convergeTol = convergeTolPixels*convergeTolPixels;

		// initialize the lines since they are used to check for corner divergence
		for (int i = 0; i < seed.size(); i++) {
			int j = (i + 1) % seed.size();
			Point2D_F64 a = seed.get(i);
			Point2D_F64 b = seed.get(j);
			UtilLine2D_F64.convert(a,b,general[i]);
		}

		boolean changed = false;
		for (int iteration = 0; iteration < maxIterations; iteration++) {
			// snap each line to the edge independently.  Lines will be in local coordinates
			for (int i = 0; i < previous.size(); i++) {
				int j = (i + 1) % previous.size();
				Point2D_F64 a = previous.get(i);
				Point2D_F64 b = previous.get(j);

				before.set(general[i]);

				boolean failed = false;
				if( !optimize(a,b,general[i]) ) {
					failed = true;
				} else {
					int k = (i+previous.size()-1) %previous.size();

					// see if the corner has diverged
					if( Intersection2D_F64.intersection(general[k], general[i],tempA) != null &&
							Intersection2D_F64.intersection(general[i], general[j],tempB) != null ) {

						if( tempA.distance(a) > maxCornerChangePixel || tempB.distance(b) > maxCornerChangePixel ) {
							failed = true;
						}
					} else {
						failed = true;
					}
				}

				// The line fit failed.  Probably because its along the image border.  Revert it
				if( failed ) {
					general[i].set(before);
				} else {
					changed = true;
				}
			}

			// Find the corners of the quadrilateral from the lines
			if( !UtilShapePolygon.convert(general,current) )
				return false;

			// see if it has converged
			boolean converged = true;
			for (int i = 0; i < current.size(); i++) {
				if( current.get(i).distance2(previous.get(i)) > convergeTol ) {
					converged = false;
					break;
				}
			}
			if( converged ) {
//				System.out.println("Converged early at "+iteration);
				break;
			} else {
				previous.set(current);
			}
		}

		return changed;
	}
955951
def map_parameter_estimate(self):
        
        res = self.pst.res
        assert res is not None
        # build the prior expectation parameter vector
        prior_expt = self.pst.parameter_data.loc[:,["parval1"]].copy()
        islog = self.pst.parameter_data.partrans == "log"
        prior_expt.loc[islog] = prior_expt.loc[islog].apply(np.log10)
        prior_expt = Matrix.from_dataframe(prior_expt)
        prior_expt.col_names = ["prior_expt"]
        # build the residual vector
        res_vec = Matrix.from_dataframe(res.loc[:,["residual"]])

        # form the terms of Schur's complement
        b = self.parcov * self.jco.T
        c = ((self.jco * self.parcov * self.jco.T) + self.obscov).inv
        bc = Matrix((b * c).x, row_names=b.row_names, col_names=c.col_names)

        # calc posterior expectation
        upgrade = bc * res_vec
        upgrade.col_names = ["prior_expt"]
        post_expt = prior_expt + upgrade

        # post processing - back log transform
        post_expt = pd.DataFrame(data=post_expt.x,index=post_expt.row_names,
                                 columns=["post_expt"])
        post_expt.loc[:,"prior_expt"] = prior_expt.x.flatten()
        post_expt.loc[islog,:] = 10.0**post_expt.loc[islog,:]
        # unc_sum = self.get_parameter_summary()
        # post_expt.loc[:,"standard_deviation"] = unc_sum.post_var.apply(np.sqrt)
        post_expt.sort_index(inplace=True)
        return post_expt
105064
private void gaussNewton( double betas[] ) {

		A_temp.reshape(L_full.numRows, numControl);
		v_temp.reshape(L_full.numRows, 1);
		x.reshape(numControl,1,false);

		// don't check numControl inside in hope that the JVM can optimize the code better
		if( numControl == 4 ) {
			for( int i = 0; i < numIterations; i++ ) {
				UtilLepetitEPnP.jacobian_Control4(L_full,betas, A_temp);
				UtilLepetitEPnP.residuals_Control4(L_full,y,betas,v_temp.data);

				if( !solver.setA(A_temp) )
					return;

				solver.solve(v_temp,x);

				for( int j = 0; j < numControl; j++ ) {
					betas[j] -= x.data[j];
				}
			}
		} else {
			for( int i = 0; i < numIterations; i++ ) {
				UtilLepetitEPnP.jacobian_Control3(L_full,betas, A_temp);
				UtilLepetitEPnP.residuals_Control3(L_full,y,betas,v_temp.data);

				if( !solver.setA(A_temp) )
					return;

				solver.solve(v_temp,x);

				for( int j = 0; j < numControl; j++ ) {
					betas[j] -= x.data[j];
				}
			}
		}
	}
161376
private int getLinearInterpolated(final int currentSamplePos, final int currentTuningPos)
	{
		final long s1 = ((long)sample[currentSamplePos  ])<<Helpers.SAMPLE_SHIFT;
		final long s2 = ((long)sample[currentSamplePos+1])<<Helpers.SAMPLE_SHIFT;
		return (int)((s1 + (((s2-s1)*((long)currentTuningPos))>>Helpers.SHIFT))>>Helpers.SAMPLE_SHIFT);
	}
706755
def admm_linearized_simple(x, f, g, L, tau, sigma, niter, **kwargs):
    
    callback = kwargs.pop('callback', None)
    z = L.range.zero()
    u = L.range.zero()
    for _ in range(niter):
        x[:] = f.proximal(tau)(x - tau / sigma * L.adjoint(L(x) + u - z))
        z = g.proximal(sigma)(L(x) + u)
        u = L(x) + u - z
        if callback is not None:
            callback(x)
113349
def optimise(self, data, initial_theta=None, full_output=False, **kwargs):
        

        data = self._format_data(data)

        if initial_theta is None:
            initial_theta = self.estimate(data)

        # Which parameters will be optimised, and which will be fixed?
        matched_channels, missing_channels, ignore_parameters \
            = self._match_channels_to_data(data)

        # For the Cannon model, the normalised fluxes are a multiplicative
        # operation, so we don't actually need to optimise the astrophysical
        # parameters, we only need to optimise the other ones. At each set of
        # non-astrophysical parameters we can determine the optimal
        # astrophysical parameters by a matrix inversion.
        parameters = [p for p in self.parameters if p not in ignore_parameters \
            and p not in self.grid_points.dtype.names]

        # What model wavelength ranges will be required?
        wavelengths_required = []
        for channel, spectrum in zip(matched_channels, data):
            if channel is None: continue
            z = initial_theta.get("z", initial_theta.get("z_{}".format(channel),
                0))
            wavelengths_required.append(
                [spectrum.disp[0] * (1 - z), spectrum.disp[-1] * (1 - z)])

        # (If necessary,..) train the Cannon model around the closest point.
        subset_bounds = self._initialise_approximator(closest_point=\
            [initial_theta[p] for p in self.grid_points.dtype.names], 
            wavelengths_required=wavelengths_required, **kwargs)
        
        # Get the optimisation keyword arguments.
        op_kwargs = self._configuration.get("optimise", {}).copy()
        op_kwargs.update(kwargs)

        # Get fixed keywords.
        fixed = op_kwargs.pop("fixed", {})
        if fixed is None: fixed = {}
        if fixed:
            # Remove non-parameters from the 'fixed' keywords.
            keys = set(fixed).intersection(self.parameters)
            ignored_keys = set(fixed).difference(parameters)
            if ignored_keys:
                logger.warn("Ignoring optimise.fixed keys because they are not"
                    " model parameters: {0}".format(", ".join(ignored_keys)))
            # If the 'fixed' value is provided, use that. Otherwise if it is
            # None then use the initial_theta value.
            fixed = dict(zip(keys, 
                [(fixed[k], initial_theta.get(k, None))[fixed[k] is None] \
                    for k in keys]))

            logger.info("Fixing keyword arguments (these will not be optimised)"
                ": {}".format(fixed))

        # Remove fixed parameters from the parameters to be optimised
        #parameters = list(set(parameters).difference(fixed))
        parameters = [p for p in parameters if p not in fixed]

        # Translate input bounds.
        nbs = (None, None) # No boundaries.
        input_bounds = op_kwargs.pop("bounds", {})
        # Because astrophysical parameters are not being 'optimised', we may not
        # actually need the subset_bounds.get call, but there may be fringe
        # cases that I cannot immediately think of.
        op_kwargs["bounds"] = [input_bounds.get(p, subset_bounds.get(p, nbs)) \
            for p in parameters]

        # Apply data masks now so we don't have to do it on the fly.
        masked_data, pixels_affected = self._apply_data_mask(data)

        # Prepare the convolution functions.
        self._create_convolution_functions(matched_channels, data, parameters)

        logger.info("Optimising parameters: {0}".format(", ".join(parameters)))
        logger.info("Optimisation keywords: {0}".format(op_kwargs))

        # Create the objective function. The objective function needs to
        # continuum-normalise the observed spectra, put it at rest, then solve
        # for the best astrophysical parameters, then *generate* that spectrum.
        debug = kwargs.get("debug", False)
        def nlp(t, return_labels=False):
            # Apply fixed keywords.
            theta = dict(zip(parameters, t))
            theta.update(fixed)

            observed_variances = []
            observed_intensities = []
            for channel, spectrum in zip(matched_channels, data):

                # For each channel:
                # 1) Put the observed spectrum at rest.
                z = theta.get("z", theta.get("z_{}".format(channel), 0))
                rest_observed_disp = spectrum.disp * (1 - z)

                # 2) Correct for continuum.
                j, coeffs = 0, []
                while theta.get("continuum_{0}_{1}".format(channel, j), None):
                    coeffs.append(theta["continuum_{0}_{1}".format(channel, j)])
                    j += 1

                # Remember: model.__call__ calculates continuum based on the
                # *observed* wavelength points, so here we do the same (e.g.,
                # not those that have potentially been corrected for redshift)
                if not coeffs: continuum = 1.0
                else:
                    continuum = np.polyval(coeffs[::-1], spectrum.disp) 
                
                # 3) Put the observed data onto the self.wavelengths scale.
                # [TODO] do the resampling correctly.
                rebinned_observed_intensities = np.interp(self.wavelengths,
                    rest_observed_disp, spectrum.flux / continuum,
                    left=np.nan, right=np.nan)

                rebinned_observed_variances = np.interp(self.wavelengths,
                    rest_observed_disp, spectrum.variance / continuum,
                    left=np.nan, right=np.nan)

                observed_variances.append(rebinned_observed_variances)
                observed_intensities.append(rebinned_observed_intensities)

            # [TODO] This may be the wrong thing to do.
            observed_variances \
                = np.nanmean(np.vstack(observed_variances), axis=0)
            observed_intensities \
                = np.nanmean(np.vstack(observed_intensities), axis=0)

            # Solve for the astrophysical parameters.
            try:
                labels = self._solve_labels(observed_intensities,
                    observed_variances)

            except:
                logger.exception("Could not determine labels:")
                if debug: raise
                return np.inf

            if return_labels:
                return labels

            # Put the optimal astrophysical parameters into our theta.
            theta.update(labels)

            # Make the log-probability call.
            # Note: The inference._ln_probability call takes a dictionary.
            return -inference._ln_probability(theta, self, data, debug,
                matched_channels=matched_channels)

        # Do the optimisation.
        p0 = np.array([initial_theta[p] for p in parameters])
        x_opt = optimise.minimise(nlp, p0, **op_kwargs)

        # Put the result into a usable form.
        result = dict(zip(parameters, x_opt))
        result.update(nlp(x_opt, return_labels=True))
        result.update(fixed)
        
        result = OrderedDict([(k, result[k]) \
            for k in self.parameters if k in result])

        if full_output:
            # Create model fluxes and calculate some metric.
            chi_sq, dof, model_fluxes = self._chi_sq(result, data)
            result = (result, chi_sq, dof, model_fluxes)

        # Remove any prepared convolution functions.
        self._destroy_convolution_functions()

        return result
416220
def updateD_H(self, x):
        
        self.precompute(x)
        H = zeros((len(x), len(x)))
        Ai = zeros(self.A.shape[0])
        Aj = zeros(Ai.shape)
        for i in range(len(x)):
            Ai = self.A[:, i]
            ti = dot(self.AD, outer(self.R[:, i], Ai)) + dot(outer(Ai, self.R[i, :]), self.ADt)

            for j in range(i, len(x)):
                Aj = self.A[:, j]
                tj = outer(Ai, Aj)
                H[i, j] = (
                    self.E * (self.R[i, j] * tj + self.R[j, i] * tj.T) -
                    ti * (
                        dot(self.AD, outer(self.R[:, j], Aj)) +
                        dot(outer(Aj, self.R[j, :]), self.ADt)
                    )
                ).sum()
                H[j, i] = H[i, j]
        H *= -2
        e = eigvals(H).min()
        H = H + (eye(H.shape[0]) * e)
        return H
327560
private void minimizeWithGeometricConstraints() {
		extractEpipoles.setTensor(solutionN);
		extractEpipoles.extractEpipoles(e2,e3);

		// encode the parameters being optimized
		param[0] = e2.x; param[1] = e2.y; param[2] = e2.z;
		param[3] = e3.x; param[4] = e3.y; param[5] = e3.z;

		// adjust the error function for the current inputs
		errorFunction.init();

		// set up the optimization algorithm
		optimizer.setFunction(errorFunction,null);
		optimizer.initialize(param,gtol,ftol);

		// optimize until convergence or the maximum number of iterations
		UtilOptimize.process(optimizer, maxIterations);

		// get the results and compute the trifocal tensor
		double found[] = optimizer.getParameters();
		paramToEpipoles(found,e2,e3);

		enforce.process(e2,e3,A);
		enforce.extractSolution(solutionN);
	}
1092591
def _initialize_model_diagnostics(self):
        """"""

        self.add_diagnostic('entspec',
            description='barotropic enstrophy spectrum',
            function= (lambda self:
                      np.abs(self.del1*self.qh[0] + self.del2*self.qh[1])**2.)
        )

        self.add_diagnostic('APEflux',
            description='spectral flux of available potential energy',
            function= (lambda self:
              self.rd**-2 * self.del1*self.del2 *
              np.real((self.ph[0]-self.ph[1])*np.conj(self.Jptpc)) )
        )

        self.add_diagnostic('KEflux',
            description='spectral flux of kinetic energy',
            function= (lambda self:
              np.real(self.del1*self.ph[0]*np.conj(self.Jpxi[0])) +
              np.real(self.del2*self.ph[1]*np.conj(self.Jpxi[1])) )
        )
        
        self.add_diagnostic('APEgenspec',
            description='spectrum of APE generation',
            function= (lambda self: self.U[:,np.newaxis] * self.rd**-2 * self.del1 * self.del2 *
                       np.real(1j*self.k*(self.del1*self.ph[0] + self.del2*self.ph[1]) *
                                  np.conj(self.ph[0] - self.ph[1])) )
        )

        self.add_diagnostic('APEgen',
            description='total APE generation',
            function= (lambda self: self.U * self.rd**-2 * self.del1 * self.del2 *
                       np.real((1j*self.k*
                            (self.del1*self.ph[0] + self.del2*self.ph[1]) *
                            np.conj(self.ph[0] - self.ph[1])).sum()
                              +(1j*self.k[:,1:-2]*
                            (self.del1*self.ph[0,:,1:-2] + self.del2*self.ph[1,:,1:-2]) *
                            np.conj(self.ph[0,:,1:-2] - self.ph[1,:,1:-2])).sum()) /
                            (self.M**2) )
        )
1091545
public boolean process( List<CalibrationObservation> observations ) {

		// compute initial parameter estimates using linear algebra
		if( !linearEstimate(observations) )
			return false;

		status("Non-linear refinement");
		// perform non-linear optimization to improve results
		if( !performBundleAdjustment())
			return false;

		return true;
	}
644048
def robust_backtrack(self):
        

        self.L *= self.L_gamma_d
        maxiter = self.L_maxiter

        iterBTrack = 0
        linesearch = 1

        self.store_Yprev()
        while linesearch and iterBTrack < maxiter:

            t = float(1. + np.sqrt(1. + 4. * self.L * self.Tk)) / (2. * self.L)
            T = self.Tk + t
            y = (self.Tk * self.var_xprv() + t * self.ZZ) / T
            self.update_var_y(y)

            gradY = self.proximal_step()  # Given Y(f), L, this updates X(f)

            f = self.obfn_f(self.var_x())
            Dxy = self.eval_Dxy()
            Q = self.obfn_f(self.var_y()) + \
                self.eval_linear_approx(Dxy, gradY) + \
                (self.L / 2.) * np.linalg.norm(Dxy.flatten(), 2)**2

            if f <= Q:
                linesearch = 0
            else:
                self.L *= self.L_gamma_u

            iterBTrack += 1

        self.Tk = T
        self.ZZ += (t * self.L * (self.var_x() - self.var_y()))

        self.F = f
        self.Q = Q
        self.iterBTrack = iterBTrack
585271
def optimise(self, data, initial_theta=None, full_output=False, **kwargs):
        

        data = self._format_data(data)

        if initial_theta is None:
            initial_theta = self.estimate(data)

        # Which parameters will be optimised, and which will be fixed?
        matched_channels, missing_channels, ignore_parameters \
            = self._match_channels_to_data(data)
        #parameters = set(self.parameters).difference(ignore_parameters)
        parameters = [p for p in self.parameters if p not in ignore_parameters]

        # What model wavelength ranges will be required?
        wavelengths_required = []
        for channel, spectrum in zip(matched_channels, data):
            if channel is None: continue
            z = initial_theta.get("z",
                initial_theta.get("z_{}".format(channel), 0))
            wavelengths_required.append(
                [spectrum.disp[0] * (1 - z), spectrum.disp[-1] * (1 - z)])

        # Create the spectrum approximator/interpolator.
        closest_point = [initial_theta[p] for p in self.grid_points.dtype.names]
        subset_bounds = self._initialise_approximator(
            closest_point=closest_point, 
            wavelengths_required=wavelengths_required, **kwargs)
        
        # Get the optimisation keyword arguments.
        op_kwargs = self._configuration.get("optimise", {}).copy()
        op_kwargs.update(kwargs)

        # Get fixed keywords.
        fixed = op_kwargs.pop("fixed", {})
        if fixed:
            # Remove non-parameters from the 'fixed' keywords.
            keys = set(fixed).intersection(parameters)
            # If the 'fixed' value is provided, use that. Otherwise if it is
            # None then use the initial_theta value.
            fixed = dict(zip(keys, 
                [(fixed[k], initial_theta.get(k, None))[fixed[k] is None] \
                    for k in keys]))

            logger.info("Fixing keyword arguments (these will not be optimised)"
                ": {}".format(fixed))

        # Remove fixed parameters from the parameters to be optimised
        #parameters = list(set(parameters).difference(fixed))
        parameters = [p for p in parameters if p not in fixed]

        # Translate input bounds.
        nbs = (None, None) # No boundaries.
        input_bounds = op_kwargs.pop("bounds", {})
        op_kwargs["bounds"] = [input_bounds.get(p, subset_bounds.get(p, nbs)) \
            for p in parameters]
        
        # Apply data masks now so we don't have to do it on the fly.
        masked_data, pixels_affected = self._apply_data_mask(data)

        # Prepare the convolution functions.
        self._create_convolution_functions(matched_channels, data, parameters,
            fixed_parameters=fixed)

        logger.info("Optimising parameters: {0}".format(", ".join(parameters)))
        logger.info("Optimisation keywords: {0}".format(op_kwargs))

        # Create the objective function.
        debug = kwargs.get("debug", False)
        def nlp(theta):
            # Apply fixed keywords
            t_ = theta.copy()
            p_ = [] + parameters
            for parameter, value in fixed.iteritems():
                p_.append(parameter)
                t_ = np.append(theta, value)

            return -inference.ln_probability(t_, p_, self, data, debug,
                matched_channels=matched_channels)

        # Do the optimisation.
        p0 = np.array([initial_theta[p] for p in parameters])

        x_opt = op.minimise(nlp, p0, **op_kwargs)

        # Put the result into a usable form.
        x_opt_theta = OrderedDict(zip(parameters, x_opt))
        x_opt_theta.update(fixed)

        if full_output:
            # Create model fluxes and calculate some metric.
            chi_sq, dof, model_fluxes = self._chi_sq(x_opt_theta, data)


            # Remove any prepared convolution functions.
            self._destroy_convolution_functions()
    
            return (x_opt_theta, chi_sq, dof, model_fluxes)

        # Remove any prepared convolution functions.
        self._destroy_convolution_functions()

        return x_opt_theta
1014780
def _initialize_stretching_matrix(self):
        

        self.S = np.zeros((self.nz, self.nz))

        if (self.nz==2) and (self.rd) and (self.delta):

            self.del1 = self.delta/(self.delta+1.)
            self.del2 = (self.delta+1.)**-1
            self.Us = self.Ubg[0]-self.Ubg[1]

            self.F1 = self.rd**-2 / (1.+self.delta)
            self.F2 = self.delta*self.F1
            self.S[0,0], self.S[0,1] = -self.F1,  self.F1
            self.S[1,0], self.S[1,1] =  self.F2, -self.F2

        else:

            for i in range(self.nz):

                if i == 0:
                    self.S[i,i]   = -self.f2/self.Hi[i]/self.gpi[i]
                    self.S[i,i+1] =  self.f2/self.Hi[i]/self.gpi[i]

                elif i == self.nz-1:
                    self.S[i,i]   = -self.f2/self.Hi[i]/self.gpi[i-1]
                    self.S[i,i-1] =  self.f2/self.Hi[i]/self.gpi[i-1]

                else:
                    self.S[i,i-1] = self.f2/self.Hi[i]/self.gpi[i-1]
                    self.S[i,i]   = -(self.f2/self.Hi[i]/self.gpi[i] +
                                        self.f2/self.Hi[i]/self.gpi[i-1])
                    self.S[i,i+1] = self.f2/self.Hi[i]/self.gpi[i]
571815
def reconstruct_emds(edm, Om, all_points, method=None, **kwargs):
    
    from .point_set import dm_from_edm
    N = all_points.shape[0]
    d = all_points.shape[1]
    dm = dm_from_edm(edm)
    if method is None:
        from .mds import superMDS
        Xhat, __ = superMDS(all_points[0, :], N, d, Om=Om, dm=dm)
    else:
        C = kwargs.get('C', None)
        b = kwargs.get('b', None)
        if C is None or b is None:
            raise NameError(
                'Need constraints C and b for reconstruct_emds in iterative mode.')
        KE_noisy = np.multiply(np.outer(dm, dm), Om)
        if method == 'iterative':
            from .mds import iterativeEMDS
            Xhat, __ = iterativeEMDS(
                all_points[0, :], N, d, KE=KE_noisy, C=C, b=b)
        elif method == 'relaxed':
            from .mds import relaxedEMDS
            Xhat, __ = relaxedEMDS(
                all_points[0, :], N, d, KE=KE_noisy, C=C, b=b)
        else:
            raise NameError('Undefined method', method)
    Y, R, t, c = procrustes(all_points, Xhat, scale=False)
    return Y
1097575
public Se3_F64 estimateNoOutliers( List<Point2D3D> observations ) {

		// Compute a single solution using EPNP
		// 10 iterations is what JavaDoc recommends, but might need to be tuned.
		// 0 test points.  This parameters is actually ignored because EPNP only returns a single solution
		Estimate1ofPnP pnp = FactoryMultiView.pnp_1(EnumPNP.EPNP, 10, 0);

		Se3_F64 worldToCamera = new Se3_F64();
		pnp.process(observations,worldToCamera);

		// For some applications the EPNP solution might be good enough, but let's refine it
		RefinePnP refine = FactoryMultiView.pnpRefine(1e-8,200);

		Se3_F64 refinedWorldToCamera = new Se3_F64();

		if( !refine.fitModel(observations,worldToCamera,refinedWorldToCamera) )
			throw new RuntimeException("Refined failed! Input probably bad...");

		return refinedWorldToCamera;
	}
73766
def iterline(x1, y1, x2, y2):
    ''
    xdiff = abs(x2-x1)
    ydiff = abs(y2-y1)
    xdir = 1 if x1 <= x2 else -1
    ydir = 1 if y1 <= y2 else -1

    r = math.ceil(max(xdiff, ydiff))
    if r == 0:  # point, not line
        yield x1, y1
    else:
        x, y = math.floor(x1), math.floor(y1)
        i = 0
        while i < r:
            x += xdir * xdiff / r
            y += ydir * ydiff / r

            yield x, y
            i += 1
70148
protected void removeNormalization( TrifocalTensor solution ) {
		DMatrixRMaj N2_inv = N2.matrixInv();
		DMatrixRMaj N3_inv = N3.matrixInv();

		DMatrixRMaj N1 = this.N1.matrix();

		for( int i = 0; i < 3; i++ ) {
			DMatrixRMaj T = solution.getT(i);
			for( int j = 0; j < 3; j++ ) {
				for( int k = 0; k < 3; k++ ) {

					double sum = 0;

					for( int r = 0; r < 3; r++ ) {
						double n1 = N1.get(r,i);
						DMatrixRMaj TN = solutionN.getT(r);
	                    for( int s = 0; s < 3; s++ ) {
							double n2 = N2_inv.get(j,s);
							for( int t = 0; t < 3; t++ ) {
								sum += n1*n2*N3_inv.get(k,t)*TN.get(s,t);
							}
						}
					}

					T.set(j,k,sum);
				}
			}
		}
	}