Specify camera calibration parameters

@param fx   Focal length x-axis in pixels
@param fy   Focal length y-axis in pixels
@param skew skew in pixels
@param cx   camera center x-axis in pixels
@param cy   center center y-axis in pixels
2098
public AddBrownPtoN_F64 setK( /**/double fx, /**/double fy, /**/double skew, /**/double cx, /**/double cy) {

		// analytic solution to matrix inverse
		a11 = (double)(1.0/fx);
		a12 = (double)(-skew/(fx*fy));
		a13 = (double)((skew*cy - cx*fy)/(fx*fy));
		a22 = (double)(1.0/fy);
		a23 = (double)(-cy/fy);

		return this;
	}
999912
public void parameterize( int x , int y , float derivX , float derivY )
	{
		// put the point in a new coordinate system centered at the image's origin
		// this minimizes error, which is a function of distance from origin
		x -= originX;
		y -= originY;

		float v = (x*derivX + y*derivY)/(derivX*derivX + derivY*derivY);

		// finds the foot a line normal equation and put the point into image coordinates
		int x0 = (int)(v*derivX) + originX;
		int y0 = (int)(v*derivY) + originY;

		if( transform.isInBounds(x0,y0)) {
			int index = transform.startIndex+y0*transform.stride+x0;
			// keep track of candidate pixels so that a sparse search can be done
			// to detect lines
			if( transform.data[index]++ == 1 )
				candidates.add(x0,y0);
		}
	}
1052333
public void configure(LensDistortionNarrowFOV distortion , Se3_F64 worldToCamera ) {
		this.worldToCamera = worldToCamera;

		normToPixel = distortion.distort_F64(false,true);
	}
1160836
def focus0(self):
        '''
        

        '''
        f = Point(self.center)

        if self.xAxisIsMajor:
            f.x -= self.linearEccentricity
        else:
            f.y -= self.linearEccentricity
        return f
609234
void computeRawDescriptor(double c_x, double c_y, double sigma, double orientation, TupleDesc_F64 descriptor) {
		double c = Math.cos(orientation);
		double s = Math.sin(orientation);

		float fwidthSubregion = widthSubregion;
		int sampleWidth = widthGrid*widthSubregion;
		double sampleRadius = sampleWidth/2;

		double sampleToPixels = sigma*sigmaToPixels;

		Deriv image = (Deriv)imageDerivX.getImage();

		for (int sampleY = 0; sampleY < sampleWidth; sampleY++) {
			float subY = sampleY/fwidthSubregion;
			double y = sampleToPixels*(sampleY-sampleRadius);

			for (int sampleX = 0; sampleX < sampleWidth; sampleX++) {
				// coordinate of samples in terms of sub-region.  Center of sample point, hence + 0.5f
				float subX = sampleX/fwidthSubregion;
				// recentered local pixel sample coordinate
				double x = sampleToPixels*(sampleX-sampleRadius);

				// pixel coordinate in the image that is to be sampled.  Note the rounding
				// If the pixel coordinate is -1 < x < 0 then it will round to 0 instead of -1, but the rounding
				// method below is WAY faster than Math.round() so this is a small loss.
				int pixelX = (int)(x*c - y*s + c_x + 0.5);
				int pixelY = (int)(x*s + y*c + c_y + 0.5);

				// skip pixels outside of the image
				if( image.isInBounds(pixelX,pixelY) ) {
					// spacial image derivative at this point
					float spacialDX = imageDerivX.unsafe_getF(pixelX, pixelY);
					float spacialDY = imageDerivY.unsafe_getF(pixelX, pixelY);

					double adjDX =  c*spacialDX + s*spacialDY;
					double adjDY = -s*spacialDX + c*spacialDY;

					double angle = UtilAngle.domain2PI(Math.atan2(adjDY,adjDX));

					float weightGaussian = gaussianWeight[sampleY*sampleWidth+sampleX];
					float weightGradient = (float)Math.sqrt(spacialDX*spacialDX + spacialDY*spacialDY);

					// trilinear interpolation intro descriptor
					trilinearInterpolation(weightGaussian*weightGradient,subX,subY,angle, descriptor);
				}
			}
		}
	}
262343
def lens_center(self, kwargs_lens, k=None, bool_list=None, numPix=200, deltaPix=0.01, center_x_init=0, center_y_init=0):
        
        x_grid, y_grid = util.make_grid(numPix=numPix, deltapix=deltaPix)
        x_grid += center_x_init
        y_grid += center_y_init

        if bool_list is None:
            kappa = self._lensModel.kappa(x_grid, y_grid, kwargs_lens, k=k)
        else:
            kappa = np.zeros_like(x_grid)
            for k in range(len(kwargs_lens)):
                if bool_list[k] is True:
                    kappa += self._lensModel.kappa(x_grid, y_grid, kwargs_lens, k=k)
        center_x = x_grid[kappa == np.max(kappa)]
        center_y = y_grid[kappa == np.max(kappa)]
        return center_x, center_y
890951
public void setCenterProperties(Point3d center) {
		this.cxProperty = center.xProperty;
		this.cyProperty = center.yProperty;
		this.czProperty = center.zProperty;
	}
475612
@Override
	public void setCenter(Point3D center) {
		this.cxProperty.set(center.getX());
		this.cyProperty.set(center.getY());
		this.czProperty.set(center.getZ());
	}
1253386
private void convertToOutput( View origin ) {
		structure = new SceneStructureMetric(false);
		observations = new SceneObservations(viewsAdded.size());

		// TODO can this be simplified?
		int idx = 0;
		for( String key : graph.cameras.keySet() ) {
			cameraToIndex.put(key,idx++);
		}

		structure.initialize(cameraToIndex.size(),viewsAdded.size(), graph.features3D.size());

		for ( String key : graph.cameras.keySet() ) {
			int i = cameraToIndex.get(key);
			structure.setCamera(i,true,graph.cameras.get(key).pinhole);
		}

		// look up table from old index to new index
		int viewOldToView[] = new int[ graph.nodes.size() ];
		Arrays.fill(viewOldToView,-1);
		for (int i = 0; i < viewsAdded.size(); i++) {
			viewOldToView[ graph.nodes.indexOf(viewsAdded.get(i))] = i;
		}

		for( int i = 0; i < viewsAdded.size(); i++ ) {
			View v = viewsAdded.get(i);
			int cameraIndex = cameraToIndex.get(v.camera.camera);
			structure.setView(i,v==origin,v.viewToWorld.invert(null));
			structure.connectViewToCamera(i,cameraIndex);
		}

		for (int indexPoint = 0; indexPoint < graph.features3D.size(); indexPoint++) {
			Feature3D f = graph.features3D.get(indexPoint);

			structure.setPoint(indexPoint,f.worldPt.x,f.worldPt.y,f.worldPt.z);

			if( f.views.size() != f.obsIdx.size )
				throw new RuntimeException("BUG!");

			for (int j = 0; j < f.views.size(); j++) {
				View view = f.views.get(j);
				int viewIndex = viewOldToView[view.index];
				structure.connectPointToView(indexPoint,viewIndex);

				Point2D_F64 pixel = viewsAdded.get(viewIndex).observationPixels.get(f.obsIdx.get(j));
				observations.getView(viewIndex).add(indexPoint,(float)(pixel.x),(float)(pixel.y));
			}
		}

	}
152943
boolean projectiveToMetric() {

		// homography from projective to metric
		DMatrixRMaj H = new DMatrixRMaj(4,4);
		listPinhole.clear();

		if( manualFocalLength <= 0 ) {
			// Estimate calibration parameters
			SelfCalibrationLinearDualQuadratic selfcalib = new SelfCalibrationLinearDualQuadratic(1.0);
			selfcalib.addCameraMatrix(P1);
			selfcalib.addCameraMatrix(P2);
			selfcalib.addCameraMatrix(P3);

			GeometricResult result = selfcalib.solve();
			if (GeometricResult.SOLVE_FAILED != result && selfcalib.getSolutions().size() == 3) {
				for (int i = 0; i < 3; i++) {
					SelfCalibrationLinearDualQuadratic.Intrinsic c = selfcalib.getSolutions().get(i);
					CameraPinhole p = new CameraPinhole(c.fx, c.fy, 0, 0, 0, width, height);
					listPinhole.add(p);
				}
			} else {
				// TODO Handle this better
				System.out.println("Self calibration failed!");
				for (int i = 0; i < 3; i++) {
					CameraPinhole p = new CameraPinhole(width / 2, width / 2, 0, 0, 0, width, height);
					listPinhole.add(p);
				}
			}
			// convert camera matrix from projective to metric
			if( !MultiViewOps.absoluteQuadraticToH(selfcalib.getQ(),H) ) {
				if( verbose != null ) {
					verbose.println("Projective to metric failed");
				}
				return false;
			}
		} else {
			// Assume all cameras have a fixed known focal length
			EstimatePlaneAtInfinityGivenK estimateV = new EstimatePlaneAtInfinityGivenK();
			estimateV.setCamera1(manualFocalLength,manualFocalLength,0,0,0);
			estimateV.setCamera2(manualFocalLength,manualFocalLength,0,0,0);

			Vector3D_F64 v = new Vector3D_F64(); // plane at infinity
			if( !estimateV.estimatePlaneAtInfinity(P2,v))
				throw new RuntimeException("Failed!");

			DMatrixRMaj K = PerspectiveOps.pinholeToMatrix(manualFocalLength,manualFocalLength,0,0,0);
			MultiViewOps.createProjectiveToMetric(K,v.x,v.y,v.z,1,H);

			for (int i = 0; i < 3; i++) {
				CameraPinhole p = new CameraPinhole(manualFocalLength,manualFocalLength, 0, 0, 0, width, height);
				listPinhole.add(p);
			}
		}

		if( verbose != null ) {
			for (int i = 0; i < 3; i++) {
				CameraPinhole r = listPinhole.get(i);
				verbose.println("fx=" + r.fx + " fy=" + r.fy + " skew=" + r.skew);
			}
			verbose.println("Projective to metric");
		}

		DMatrixRMaj K = new DMatrixRMaj(3,3);

		// ignore K since we already have that
		MultiViewOps.projectiveToMetric(P1,H,worldToView.get(0),K);
		MultiViewOps.projectiveToMetric(P2,H,worldToView.get(1),K);
		MultiViewOps.projectiveToMetric(P3,H,worldToView.get(2),K);

		// scale is arbitrary. Set max translation to 1
		double maxT = 0;
		for( Se3_F64 p : worldToView ) {
			maxT = Math.max(maxT,p.T.norm());
		}
		for( Se3_F64 p : worldToView ) {
			p.T.scale(1.0/maxT);
			if( verbose != null ) {
				verbose.println(p);
			}
		}

		return true;
	}
611766
def calibrate_cameras(self):
        """"""
        criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS,
                    100, 1e-5)
        flags = (cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_ZERO_TANGENT_DIST +
                 cv2.CALIB_SAME_FOCAL_LENGTH)
        calib = StereoCalibration()
        (calib.cam_mats["left"], calib.dist_coefs["left"],
         calib.cam_mats["right"], calib.dist_coefs["right"],
         calib.rot_mat, calib.trans_vec, calib.e_mat,
         calib.f_mat) = cv2.stereoCalibrate(self.object_points,
                                            self.image_points["left"],
                                            self.image_points["right"],
                                            self.image_size,
                                            calib.cam_mats["left"],
                                            calib.dist_coefs["left"],
                                            calib.cam_mats["right"],
                                            calib.dist_coefs["right"],
                                            calib.rot_mat,
                                            calib.trans_vec,
                                            calib.e_mat,
                                            calib.f_mat,
                                            criteria=criteria,
                                            flags=flags)[1:]
        (calib.rect_trans["left"], calib.rect_trans["right"],
         calib.proj_mats["left"], calib.proj_mats["right"],
         calib.disp_to_depth_mat, calib.valid_boxes["left"],
         calib.valid_boxes["right"]) = cv2.stereoRectify(calib.cam_mats["left"],
                                                      calib.dist_coefs["left"],
                                                      calib.cam_mats["right"],
                                                      calib.dist_coefs["right"],
                                                      self.image_size,
                                                      calib.rot_mat,
                                                      calib.trans_vec,
                                                      flags=0)
        for side in ("left", "right"):
            (calib.undistortion_map[side],
             calib.rectification_map[side]) = cv2.initUndistortRectifyMap(
                                                        calib.cam_mats[side],
                                                        calib.dist_coefs[side],
                                                        calib.rect_trans[side],
                                                        calib.proj_mats[side],
                                                        self.image_size,
                                                        cv2.CV_32FC1)
        # This is replaced because my results were always bad. Estimates are
        # taken from the OpenCV samples.
        width, height = self.image_size
        focal_length = 0.8 * width
        calib.disp_to_depth_mat = np.float32([[1, 0, 0, -0.5 * width],
                                              [0, -1, 0, 0.5 * height],
                                              [0, 0, 0, -focal_length],
                                              [0, 0, 1, 0]])
        return calib
965872
public void adjustOffsets(final IGeoPoint pGeoPoint, final PointF pPixel) {
		if (pPixel == null) {
			return;
		}
		final Point unRotatedExpectedPixel = unrotateAndScalePoint((int)pPixel.x, (int)pPixel.y, null);
		final Point unRotatedActualPixel = toPixels(pGeoPoint, null);
		final long deltaX = unRotatedExpectedPixel.x - unRotatedActualPixel.x;
		final long deltaY = unRotatedExpectedPixel.y - unRotatedActualPixel.y;
		adjustOffsets(deltaX, deltaY);
	}
894485
public static BufferedImage rotate(BufferedImage image, double angle, int cx, int cy) {
        int width = image.getWidth(null);
        int height = image.getHeight(null);

        int minX, minY, maxX, maxY;
        minX = minY = maxX = maxY = 0;

        int[] corners = {0, 0, width, 0, width, height, 0, height};

        double theta = Math.toRadians(angle);
        for (int i = 0; i < corners.length; i += 2) {
            int x = (int) (Math.cos(theta) * (corners[i] - cx)
                    - Math.sin(theta) * (corners[i + 1] - cy) + cx);
            int y = (int) (Math.sin(theta) * (corners[i] - cx)
                    + Math.cos(theta) * (corners[i + 1] - cy) + cy);

            if (x > maxX) {
                maxX = x;
            }

            if (x < minX) {
                minX = x;
            }

            if (y > maxY) {
                maxY = y;
            }

            if (y < minY) {
                minY = y;
            }

        }

        cx = (cx - minX);
        cy = (cy - minY);

        BufferedImage bi = new BufferedImage((maxX - minX), (maxY - minY),
                image.getType());
        Graphics2D g2 = bi.createGraphics();
        g2.setRenderingHint(RenderingHints.KEY_INTERPOLATION,
                RenderingHints.VALUE_INTERPOLATION_BICUBIC);

        g2.setBackground(Color.white);
        g2.fillRect(0, 0, bi.getWidth(), bi.getHeight());

        AffineTransform at = new AffineTransform();
        at.rotate(theta, cx, cy);

        g2.setTransform(at);
        g2.drawImage(image, -minX, -minY, null);
        g2.dispose();

        return bi;
    }
578370
@Override
	public void setCenter(double cx, double cy, double cz) {
		this.center.set(cx, cy, cz);
	}
310653
private void process(int[] srcImage, int[] dstImage, int x, int y)
    {
        final int b = getSourcePixel(srcImage, x, y - 1);
        final int d = getSourcePixel(srcImage, x - 1, y);
        final int e = getSourcePixel(srcImage, x, y);
        final int f = getSourcePixel(srcImage, x + 1, y);
        final int h = getSourcePixel(srcImage, x, y + 1);
        int e0 = e;
        int e1 = e;
        int e2 = e;
        int e3 = e;
        if (b != h && d != f)
        {
            e0 = d == b ? d : e;
            e1 = b == f ? f : e;
            e2 = d == h ? d : e;
            e3 = h == f ? f : e;
        }

        setDestPixel(dstImage, x * SCALE, y * SCALE, e0);
        setDestPixel(dstImage, x * SCALE + 1, y * SCALE, e1);
        setDestPixel(dstImage, x * SCALE, y * SCALE + 1, e2);
        setDestPixel(dstImage, x * SCALE + 1, y * SCALE + 1, e3);
    }
452438
function (x, y) {
            // reset the initial camera position to 0,0
            this.pos.x = x || 0;
            this.pos.y = y || 0;

            // reset the target
            this.unfollow();

            // damping default value
            this.smoothFollow = true;
            this.damping = 1.0;

            // reset the transformation matrix
            this.currentTransform.identity();
        }
269932
@Override
	public boolean process(double x, double y) {

		leftPixelToRect.compute(x,y,pixelRect);

		// round to the nearest pixel
		if( !disparity.process((int)(pixelRect.x+0.5),(int)(pixelRect.y+0.5)) )
			return false;

		// Compute coordinate in camera frame
		this.w = disparity.getDisparity();
		computeHomo3D(pixelRect.x, pixelRect.y, pointLeft);

		return true;
	}