Internal function which applies the rectification adjustment to a calibrated stereo pair
177536
private static void adjustCalibrated(DMatrixRMaj rectifyLeft, DMatrixRMaj rectifyRight,
										 DMatrixRMaj rectifyK,
										 RectangleLength2D_F64 bound, double scale) {
		// translation
		double deltaX = -bound.x0*scale;
		double deltaY = -bound.y0*scale;

		// adjustment matrix
		SimpleMatrix A = new SimpleMatrix(3,3,true,new double[]{scale,0,deltaX,0,scale,deltaY,0,0,1});
		SimpleMatrix rL = SimpleMatrix.wrap(rectifyLeft);
		SimpleMatrix rR = SimpleMatrix.wrap(rectifyRight);
		SimpleMatrix K = SimpleMatrix.wrap(rectifyK);

		// remove previous calibration matrix
		SimpleMatrix K_inv = K.invert();
		rL = K_inv.mult(rL);
		rR = K_inv.mult(rR);

		// compute new calibration matrix and apply it
		K = A.mult(K);

		rectifyK.set(K.getDDRM());
		rectifyLeft.set(K.mult(rL).getDDRM());
		rectifyRight.set(K.mult(rR).getDDRM());
	}
185707
private boolean extractCalibration(DMatrixRMaj x , CameraPinhole calibration) {
		double s = x.data[5];

		double cx = calibration.cx = x.data[2]/s;
		double cy = calibration.cy = x.data[4]/s;
		double fy = calibration.fy = Math.sqrt(x.data[3]/s-cy*cy);
		double sk = calibration.skew = (x.data[1]/s-cx*cy)/fy;
		calibration.fx = Math.sqrt(x.data[0]/s - sk*sk - cx*cx);

		if( calibration.fx < 0 || calibration.fy < 0 )
			return false;
		if(UtilEjml.isUncountable(fy) || UtilEjml.isUncountable(calibration.fx))
			return false;
		if(UtilEjml.isUncountable(sk))
			return false;
		return true;
	}
1274729
def _load_calib(self):
        """"""
        # We'll build the calibration parameters as a dictionary, then
        # convert it to a namedtuple to prevent it from being modified later
        data = {}

        # Load the rigid transformation from IMU to velodyne
        data['T_velo_imu'] = self._load_calib_rigid('calib_imu_to_velo.txt')

        # Load the camera intrinsics and extrinsics
        data.update(self._load_calib_cam_to_cam(
            'calib_velo_to_cam.txt', 'calib_cam_to_cam.txt'))

        # Pre-compute the IMU to rectified camera coordinate transforms
        data['T_cam0_imu'] = data['T_cam0_velo'].dot(data['T_velo_imu'])
        data['T_cam1_imu'] = data['T_cam1_velo'].dot(data['T_velo_imu'])
        data['T_cam2_imu'] = data['T_cam2_velo'].dot(data['T_velo_imu'])
        data['T_cam3_imu'] = data['T_cam3_velo'].dot(data['T_velo_imu'])

        self.calib = namedtuple('CalibData', data.keys())(*data.values())
197535
public static void computeCameraControl(double beta[],
											List<Point3D_F64> nullPts[],
											FastQueue<Point3D_F64> cameraPts ,
											int numControl )
	{
		cameraPts.reset();
		for( int i = 0; i < numControl; i++ ) {
			cameraPts.grow().set(0,0,0);
		}

		for( int i = 0; i < numControl; i++ ) {
			double b = beta[i];
//			System.out.printf("%7.3f ", b);

			for( int j = 0; j < numControl; j++ ) {
				Point3D_F64 s = cameraPts.get(j);
				Point3D_F64 p = nullPts[i].get(j);
				s.x += b*p.x;
				s.y += b*p.y;
				s.z += b*p.z;
			}
		}
	}
1073393
public boolean estimate(List<Homography2D_F64> viewsI_to_view0 , CameraPinhole calibration ) {
		singular = false;
		int N = viewsI_to_view0.size();

		ensureDeterminantOfOne(viewsI_to_view0);

		A.reshape(N*6,6);

		for (int i = 0; i < N; i++) {
			add(i,viewsI_to_view0.get(i),A);
		}

		// Compute the SVD for its null space
		if( !svd.decompose(A)) {
			return false;
		}

		DMatrixRMaj nv = new DMatrixRMaj(6,1);
		SingularOps_DDRM.nullVector(svd,true,nv);

		if( !extractCalibration(nv,calibration) )
			return false;

		// determine if the solution is good by looking at two smallest singular values
		// If there isn't a steep drop it either isn't singular or more there is more than 1 singular value
		double sv[] = svd.getSingularValues();
		Arrays.sort(sv);
		if( singularThreshold*sv[1] <= sv[0] )  {
			return false;
		}
		return true;
	}
337641
def stash_calibration(self, attenuations, freqs, frange, calname):
        """"""
        self.calibration_vector = attenuations
        self.calibration_freqs = freqs
        self.calibration_frange = frange
        self.calname = calname
1020783
private void calcZDirection() {
		calcBoundaries();

		// if the longer part of the structure faces towards the back (-z direction),
		// rotate around y-axis so the longer part faces the viewer (+z direction)
		if (Math.abs(minBoundary.z) > Math.abs(maxBoundary.z)) {
			Matrix4d rot = flipY();
			rot.mul(transformationMatrix);
			transformationMatrix.set(rot);
		}
	}
154927
function rectPoint(progress) {
  if (progress <= 1 / 8) {
    return [1, 0.5 + progress * 4];
  }
  if (progress <= 3 / 8) {
    return [1.5 - 4 * progress, 1];
  }
  if (progress <= 5 / 8) {
    return [0, 2.5 - 4 * progress];
  }
  if (progress <= 7 / 8) {
    return [4 * progress - 2.5, 0];
  }
  return [1, 4 * progress - 3.5];
}
569654
def adjust_to_360(val, key):
    
    CheckDec = ['_dec', '_lon', '_azimuth', 'dip_direction']
    adjust = False
    for dec_key in CheckDec:
        if dec_key in key:
            if key.endswith(dec_key) or key.endswith('_'):
                adjust = True
    if not val:
        return ''
    elif not adjust:
        return val
    elif adjust:
        new_val = float(val) % 360
        if new_val != float(val):
            print('-I- adjusted {} {} to 0=>360.: {}'.format(key, val, new_val))
        return new_val
459894
private void findBestValidSolution(BundleAdjustment<SceneStructureMetric> bundleAdjustment) {
		// prints out useful debugging information that lets you know how well it's converging
		if( verbose != null && verboseLevel > 0 )
			bundleAdjustment.setVerbose(verbose,0);

		// Specifies convergence criteria
		bundleAdjustment.configure(convergeSBA.ftol, convergeSBA.gtol, convergeSBA.maxIterations);

		bundleAdjustment.setParameters(structure,observations);
		bundleAdjustment.optimize(structure);

		// ensure that the points are in front of the camera and are a valid solution
		if( checkBehindCamera(structure) ) {
			if( verbose != null )
				verbose.println("  flipping view");
			flipAround(structure,observations);
			bundleAdjustment.setParameters(structure,observations);
			bundleAdjustment.optimize(structure);
		}

		double bestScore = bundleAdjustment.getFitScore();
		List<Se3_F64> bestPose = new ArrayList<>();
		List<BundlePinholeSimplified> bestCameras = new ArrayList<>();
		for (int i = 0; i < structure.views.length; i++) {
			BundlePinholeSimplified c = structure.cameras[i].getModel();
			bestPose.add(structure.views[i].worldToView.copy());
			bestCameras.add( c.copy());
		}

		for (int i = 0; i < structure.cameras.length; i++) {
			BundlePinholeSimplified c = structure.cameras[i].getModel();
			c.f = listPinhole.get(i).fx;
			c.k1 = c.k2 = 0;
		}
		// flip rotation assuming that it was done wrong
		for (int i = 1; i < structure.views.length; i++) {
			CommonOps_DDRM.transpose(structure.views[i].worldToView.R);
		}
		triangulatePoints(structure,observations);

		bundleAdjustment.setParameters(structure,observations);
		bundleAdjustment.optimize(structure);

		if( checkBehindCamera(structure) ) {
			if( verbose != null )
				verbose.println("  flipping view");
			flipAround(structure,observations);
			bundleAdjustment.setParameters(structure,observations);
			bundleAdjustment.optimize(structure);
		}

		// revert to old settings
		if( verbose != null )
			verbose.println(" ORIGINAL / NEW = " + bestScore+" / "+bundleAdjustment.getFitScore());
		if( bundleAdjustment.getFitScore() > bestScore ) {
			if( verbose != null )
				verbose.println("  recomputing old structure");
			for (int i = 0; i < structure.cameras.length; i++) {
				BundlePinholeSimplified c = structure.cameras[i].getModel();
				c.set(bestCameras.get(i));
				structure.views[i].worldToView.set(bestPose.get(i));
			}
			triangulatePoints(structure,observations);
			bundleAdjustment.setParameters(structure,observations);
			bundleAdjustment.optimize(structure);
			if( verbose != null )
				verbose.println("  score = "+bundleAdjustment.getFitScore());
		}
	}
826864
public void convertIntoBundleStructure(List<Se3_F64> motions,
										   DMatrixRMaj K,
										   double[] distort,
										   List<CalibrationObservation> obs ) {

		structure = new SceneStructureMetric(false);
		observations = new SceneObservations(motions.size(),true);

		structure.initialize(1,motions.size(),layout.size(),1);

		// A single camera is assumed, that's what is being calibrated!
		structure.setCamera(0,false,cameraGenerator.initalizeCamera(K,distort));
		// A single rigid planar target is being viewed. It is assumed to be centered at the origin
		structure.setRigid(0,true,new Se3_F64(),layout.size());
		// Where the points are on the calibration target
		SceneStructureMetric.Rigid rigid = structure.rigids[0];
		for (int i = 0; i < layout.size(); i++) {
			rigid.setPoint(i,layout.get(i).x,layout.get(i).y,0);
		}

		// Add the initial estimate of each view's location and the points observed
		for (int viewIdx = 0; viewIdx < motions.size(); viewIdx++) {
			structure.setView(viewIdx,false,motions.get(viewIdx));
			SceneObservations.View v = observations.getViewRigid(viewIdx);
			structure.connectViewToCamera(viewIdx,0);
			CalibrationObservation ca = obs.get(viewIdx);
			for (int j = 0; j < ca.size(); j++) {
				PointIndex2D_F64 p = ca.get(j);
				v.add(p.index, (float)p.x, (float)p.y);
				structure.connectPointToView(p.index,viewIdx);
			}
		}
	}
824065
def dImage_wrt_2dVerts_bnd(observed, visible, visibility, barycentric, image_width, image_height, num_verts, f, bnd_bool):
    """"""

    n_channels = np.atleast_3d(observed).shape[2]
    shape = visibility.shape

    # Step 1: get the structure ready, ie the IS and the JS
    IS = np.tile(col(visible), (1, 2*f.shape[1])).ravel()
    JS = col(f[visibility.ravel()[visible]].ravel())
    JS = np.hstack((JS*2, JS*2+1)).ravel()

    pxs = np.asarray(visible % shape[1], np.int32)
    pys = np.asarray(np.floor(np.floor(visible) / shape[1]), np.int32)

    if n_channels > 1:
        IS = np.concatenate([IS*n_channels+i for i in range(n_channels)])
        JS = np.concatenate([JS for i in range(n_channels)])

    # Step 2: get the data ready, ie the actual values of the derivatives
    ksize = 1
    bndf = bnd_bool.astype(np.float64)
    nbndf = np.logical_not(bnd_bool).astype(np.float64)
    sobel_normalizer = cv2.Sobel(np.asarray(np.tile(row(np.arange(10)), (10, 1)), np.float64), cv2.CV_64F, dx=1, dy=0, ksize=ksize)[5,5]

    bnd_nan = bndf.reshape((observed.shape[0], observed.shape[1], -1)).copy()
    bnd_nan.ravel()[bnd_nan.ravel()>0] = np.nan
    bnd_nan += 1
    obs_nonbnd = np.atleast_3d(observed) * bnd_nan

    ydiffnb, xdiffnb = nangradients(obs_nonbnd)

    observed = np.atleast_3d(observed)

    if observed.shape[2] > 1:
        ydiffbnd, xdiffbnd, _ = np.gradient(observed)
    else:
        ydiffbnd, xdiffbnd = np.gradient(observed.squeeze())
        ydiffbnd = np.atleast_3d(ydiffbnd)
        xdiffbnd = np.atleast_3d(xdiffbnd)

    # This corrects for a bias imposed boundary differences begin spread over two pixels
    # (by np.gradients or similar) but only counted once (since OpenGL's line
    # drawing spans 1 pixel)
    xdiffbnd *= 2.0
    ydiffbnd *= 2.0

    xdiffnb = -xdiffnb
    ydiffnb = -ydiffnb
    xdiffbnd = -xdiffbnd
    ydiffbnd = -ydiffbnd
    # ydiffnb *= 0
    # xdiffnb *= 0

    if False:
        import matplotlib.pyplot as plt
        plt.figure()
        plt.subplot(121)
        plt.imshow(xdiffnb)
        plt.title('xdiffnb')
        plt.subplot(122)
        plt.imshow(xdiffbnd)
        plt.title('xdiffbnd')
        import pdb; pdb.set_trace()

    idxs = np.isnan(xdiffnb.ravel())
    xdiffnb.ravel()[idxs] = xdiffbnd.ravel()[idxs]

    idxs = np.isnan(ydiffnb.ravel())
    ydiffnb.ravel()[idxs] = ydiffbnd.ravel()[idxs]

    if True: # should be right thing
        xdiff = xdiffnb
        ydiff = ydiffnb
    else:  #should be old way
        xdiff = xdiffbnd
        ydiff = ydiffbnd


    # TODO: NORMALIZER IS WRONG HERE
    # xdiffnb = -cv2.Sobel(obs_nonbnd, cv2.CV_64F, dx=1, dy=0, ksize=ksize) / np.atleast_3d(cv2.Sobel(row(np.arange(obs_nonbnd.shape[1])).astype(np.float64), cv2.CV_64F, dx=1, dy=0, ksize=ksize))
    # ydiffnb = -cv2.Sobel(obs_nonbnd, cv2.CV_64F, dx=0, dy=1, ksize=ksize) / np.atleast_3d(cv2.Sobel(col(np.arange(obs_nonbnd.shape[0])).astype(np.float64), cv2.CV_64F, dx=0, dy=1, ksize=ksize))
    #
    # xdiffnb.ravel()[np.isnan(xdiffnb.ravel())] = 0.
    # ydiffnb.ravel()[np.isnan(ydiffnb.ravel())] = 0.
    # xdiffnb.ravel()[np.isinf(xdiffnb.ravel())] = 0.
    # ydiffnb.ravel()[np.isinf(ydiffnb.ravel())] = 0.

    # xdiffnb = np.atleast_3d(xdiffnb)
    # ydiffnb = np.atleast_3d(ydiffnb)
    #
    # xdiffbnd = -cv2.Sobel(observed, cv2.CV_64F, dx=1, dy=0, ksize=ksize) / sobel_normalizer
    # ydiffbnd = -cv2.Sobel(observed, cv2.CV_64F, dx=0, dy=1, ksize=ksize) / sobel_normalizer
    #
    # xdiff = xdiffnb * np.atleast_3d(nbndf)
    # xdiff.ravel()[np.isnan(xdiff.ravel())] = 0
    # xdiff += xdiffbnd*np.atleast_3d(bndf)
    #
    # ydiff = ydiffnb * np.atleast_3d(nbndf)
    # ydiff.ravel()[np.isnan(ydiff.ravel())] = 0
    # ydiff += ydiffbnd*np.atleast_3d(bndf)

    #import pdb; pdb.set_trace()

    #xdiff = xdiffnb
    #ydiff = ydiffnb

    #import pdb; pdb.set_trace()

    datas = []

    # The data is weighted according to barycentric coordinates
    bc0 = col(barycentric[pys, pxs, 0])
    bc1 = col(barycentric[pys, pxs, 1])
    bc2 = col(barycentric[pys, pxs, 2])
    for k in range(n_channels):
        dxs = xdiff[pys, pxs, k]
        dys = ydiff[pys, pxs, k]
        if f.shape[1] == 3:
            datas.append(np.hstack((col(dxs)*bc0,col(dys)*bc0,col(dxs)*bc1,col(dys)*bc1,col(dxs)*bc2,col(dys)*bc2)).ravel())
        else:
            datas.append(np.hstack((col(dxs)*bc0,col(dys)*bc0,col(dxs)*bc1,col(dys)*bc1)).ravel())

    data = np.concatenate(datas)

    ij = np.vstack((IS.ravel(), JS.ravel()))
    result = sp.csc_matrix((data, ij), shape=(image_width*image_height*n_channels, num_verts*2))

    return result
1140333
public static void applyDistortion(Point2D_F64 normPt, double[] radial, double t1 , double t2 )
	{
		final double x = normPt.x;
		final double y = normPt.y;

		double a = 0;
		double r2 = x*x + y*y;
		double r2i = r2;
		for( int i = 0; i < radial.length; i++ ) {
			a += radial[i]*r2i;
			r2i *= r2;
		}

		normPt.x = x + x*a + 2*t1*x*y + t2*(r2 + 2*x*x);
		normPt.y = y + y*a + t1*(r2 + 2*y*y) + 2*t2*x*y;
	}
454065
private static void adjustUncalibrated(DMatrixRMaj rectifyLeft, DMatrixRMaj rectifyRight,
										   RectangleLength2D_F64 bound, double scale) {
		// translation
		double deltaX = -bound.x0*scale;
		double deltaY = -bound.y0*scale;

		// adjustment matrix
		SimpleMatrix A = new SimpleMatrix(3,3,true,new double[]{scale,0,deltaX,0,scale,deltaY,0,0,1});
		SimpleMatrix rL = SimpleMatrix.wrap(rectifyLeft);
		SimpleMatrix rR = SimpleMatrix.wrap(rectifyRight);

		rectifyLeft.set(A.mult(rL).getDDRM());
		rectifyRight.set(A.mult(rR).getDDRM());
	}
241974
def tune_pair(self, pair):
        """"""
        self._save_bm_state()
        self.pair = pair
        self.update_disparity_map()
1200454
private boolean estimateMotion() {
		modelFitData.reset();

		Point2D_F64 normLeft = new Point2D_F64();
		Point2D_F64 normRight = new Point2D_F64();

		// use 0 -> 1 stereo associations to estimate each feature's 3D position
		for( int i = 0; i < quadViews.size; i++ ) {
			QuadView obs = quadViews.get(i);

			// convert old stereo view to normalized coordinates
			leftImageToNorm.compute(obs.v0.x,obs.v0.y,normLeft);
			rightImageToNorm.compute(obs.v1.x,obs.v1.y,normRight);

			// compute 3D location using triangulation
			triangulate.triangulate(normLeft,normRight,leftToRight,obs.X);

			// add to data set for fitting if not at infinity
			if( !Double.isInfinite(obs.X.normSq()) ) {
				Stereo2D3D data = modelFitData.grow();
				leftImageToNorm.compute(obs.v2.x,obs.v2.y,data.leftObs);
				rightImageToNorm.compute(obs.v3.x,obs.v3.y,data.rightObs);
				data.location.set(obs.X);
			}
		}

		// robustly match the data
		if( !matcher.process(modelFitData.toList()) )
			return false;

		Se3_F64 oldToNew = matcher.getModelParameters();

//		System.out.println("matcher rot = "+toString(oldToNew));
		// optionally refine the results
		if( modelRefiner != null ) {
			Se3_F64 found = new Se3_F64();
			if( modelRefiner.fitModel(matcher.getMatchSet(), oldToNew, found) ) {
//				System.out.println("matcher rot = "+toString(found));
				found.invert(newToOld);
			} else {
				oldToNew.invert(newToOld);
//				System.out.println("Fit failed!");
			}
		} else {
			oldToNew.invert(newToOld);
		}

		// compound the just found motion with the previously found motion
		Se3_F64 temp = new Se3_F64();
		newToOld.concat(leftCamToWorld, temp);
		leftCamToWorld.set(temp);

		return true;
	}
645749
public static List<AssociatedPair> convertToNormalizedCoordinates(List<AssociatedPair> matchedFeatures, CameraPinholeBrown intrinsic) {

		Point2Transform2_F64 p_to_n = LensDistortionFactory.narrow(intrinsic).undistort_F64(true, false);

		List<AssociatedPair> calibratedFeatures = new ArrayList<>();

		for (AssociatedPair p : matchedFeatures) {
			AssociatedPair c = new AssociatedPair();

			p_to_n.compute(p.p1.x, p.p1.y, c.p1);
			p_to_n.compute(p.p2.x, p.p2.y, c.p2);

			calibratedFeatures.add(c);
		}

		return calibratedFeatures;
	}