boofcv.alg.geo.rectify.RectifyCalibrated Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of boofcv-geo Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
There is a newer version: 1.1.7
/*
 * Copyright (c) 2021, Peter Abeles. All Rights Reserved.
 *
 * This file is part of BoofCV (http://boofcv.org).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package boofcv.alg.geo.rectify;

import georegression.geometry.GeometryMath_F64;
import georegression.struct.point.Vector3D_F64;
import georegression.struct.se.Se3_F64;
import lombok.Getter;
import org.ejml.data.DMatrixRMaj;
import org.ejml.simple.SimpleMatrix;

/**
 * 
 * Rectifies a stereo pair with known camera calibration using a simple algorithm described in [1]
 * such that the epipoles project to infinity along the x-axis. After rectification both images will
 * have the same intrinsic calibration matrix, same extrinsic rotation matrix, but the optical centers
 * are not modified.
 * 
 *
 * 
 * The calibration matrix is the standard upper triangular matrix used throughout the library. A single
 * calibration matrix is computed for both images by averaging the two original and setting the skew
 * to zero.
 * 
 *
 * 
 * The rectified view is chosen such that it will be most similar to the first camera. This is done by making
 * the original z-axis and the rectified z-axis similar.
 * 
 *
 * 
 * [1] A. Fusiello, E. Trucco, and A. Verri, "A Compact Algorithm for Rectification of Stereo Pairs"
 * Machine Vision and Applications, 2000
 * 
 *
 * @author Peter Abeles
 */
@SuppressWarnings({"NullAway.Init"})
public class RectifyCalibrated {

	/** Rectification transform for view-1 camera. Undistored to rectified pixels. */
	final @Getter DMatrixRMaj undistToRectPixels1 = new DMatrixRMaj(3, 3);
	/** Rectification transform for view-2 camera. Undistored to rectified pixels. */
	final @Getter DMatrixRMaj undistToRectPixels2 = new DMatrixRMaj(3, 3);

	// direction of new coordinate system axises
	final Vector3D_F64 v1 = new Vector3D_F64();
	final Vector3D_F64 v2 = new Vector3D_F64();
	final Vector3D_F64 v3 = new Vector3D_F64();

	// Camera calibration matrix.
	final SimpleMatrix K = new SimpleMatrix(3, 3);

	/**
	 * Rotation matrix of rectified coordinate system. To convert back into left camera reference frame multiply
	 * the triangulated point by the transpose of this matrix
	 */
	@Getter DMatrixRMaj rectifiedRotation;

	/**
	 * Computes rectification transforms for both cameras and optionally a single calibration
	 * matrix.
	 *
	 * @param K1 Calibration matrix for first camera.
	 * @param worldToCamera1 Location of the first camera.
	 * @param K2 Calibration matrix for second camera.
	 * @param worldToCamera2 Location of the second camera.
	 */
	public void process( DMatrixRMaj K1, Se3_F64 worldToCamera1,
						 DMatrixRMaj K2, Se3_F64 worldToCamera2 ) {
		SimpleMatrix sK1 = SimpleMatrix.wrap(K1);
		SimpleMatrix sK2 = SimpleMatrix.wrap(K2);
		SimpleMatrix R1 = SimpleMatrix.wrap(worldToCamera1.getR());
		SimpleMatrix R2 = SimpleMatrix.wrap(worldToCamera2.getR());
		SimpleMatrix T1 = new SimpleMatrix(3, 1, true,
				new double[]{worldToCamera1.getT().x, worldToCamera1.getT().y, worldToCamera1.getT().z});
		SimpleMatrix T2 = new SimpleMatrix(3, 1, true,
				new double[]{worldToCamera2.getT().x, worldToCamera2.getT().y, worldToCamera2.getT().z});

		//  P = K*[R|T]
		SimpleMatrix KR1 = sK1.mult(R1);
		SimpleMatrix KR2 = sK2.mult(R2);

		// compute optical centers in world reference frame
		// c = -R'*T
		SimpleMatrix c1 = R1.transpose().mult(T1.scale(-1));
		SimpleMatrix c2 = R2.transpose().mult(T2.scale(-1));

		// new coordinate system axises
		selectAxises(R1, R2, c1, c2);

		// new extrinsic parameters, rotation matrix with rows of camera 1's coordinate system in
		// the world frame
		SimpleMatrix RR = new SimpleMatrix(3, 3, true,
				new double[]{
						v1.x, v1.y, v1.z,
						v2.x, v2.y, v2.z,
						v3.x, v3.y, v3.z});

		// new calibration matrix that is an average of the original
		K.setTo(sK1.plus(sK2).scale(0.5));
		K.set(0, 1, 0);// set skew to zero

		// new projection rotation matrices
		SimpleMatrix KRR = K.mult(RR);

		// rectification transforms
		undistToRectPixels1.setTo(KRR.mult(KR1.invert()).getDDRM());
		undistToRectPixels2.setTo(KRR.mult(KR2.invert()).getDDRM());

		rectifiedRotation = RR.getDDRM();
	}

	/**
	 * Selects axises of new coordinate system
	 */
	private void selectAxises( SimpleMatrix R1, SimpleMatrix R2, SimpleMatrix c1, SimpleMatrix c2 ) {
		// --------- Compute the new x-axis
		v1.setTo(c2.get(0) - c1.get(0), c2.get(1) - c1.get(1), c2.get(2) - c1.get(2));
		v1.normalize();

		// --------- Compute the new y-axis
		//   cross product of old z axis and new x axis
		// According to the paper [1] this choice is arbitrary, however it is not. By selecting
		// the original axis the similarity with the first view is maximized. The other extreme
		// would be to make it perpendicular, resulting in an unusable rectification.

		// extract old z-axis from rotation matrix
		Vector3D_F64 oldZ = new Vector3D_F64(
				R1.get(2, 0) + R2.get(2, 0),
				R1.get(2, 1) + R2.get(2, 1),
				R1.get(2, 2) + R2.get(2, 2));
		GeometryMath_F64.cross(oldZ, v1, v2);
		v2.normalize();

		// ---------- Compute the new z-axis
		// simply the process product of the first two
		GeometryMath_F64.cross(v1, v2, v3);
		v3.normalize();
	}

	/**
	 * If a single calibration matrix was requested then this returns it.
	 *
	 * @return Calibration matrix for both cameras
	 */
	public DMatrixRMaj getCalibrationMatrix() {
		return K.getDDRM();
	}
}