boofcv.alg.distort.spherical.MultiCameraToEquirectangular Maven / Gradle / Ivy

Go to download
/*
 * Copyright (c) 2011-2018, 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.distort.spherical;

import boofcv.alg.distort.ImageDistort;
import boofcv.alg.distort.LensDistortionWideFOV;
import boofcv.alg.distort.PixelTransformCached_F32;
import boofcv.alg.distort.PointToPixelTransform_F32;
import boofcv.alg.misc.GImageMiscOps;
import boofcv.alg.misc.GPixelMath;
import boofcv.alg.misc.PixelMath;
import boofcv.struct.distort.PixelTransform2_F32;
import boofcv.struct.distort.Point2Transform2_F32;
import boofcv.struct.distort.Point2Transform3_F32;
import boofcv.struct.distort.Point3Transform2_F32;
import boofcv.struct.image.GrayF32;
import boofcv.struct.image.GrayU8;
import boofcv.struct.image.ImageBase;
import boofcv.struct.image.ImageType;
import georegression.geometry.GeometryMath_F32;
import georegression.geometry.UtilVector3D_F32;
import georegression.metric.UtilAngle;
import georegression.struct.point.Point2D_F32;
import georegression.struct.point.Point3D_F32;
import georegression.struct.se.Se3_F32;
import org.ejml.UtilEjml;
import org.ejml.data.FMatrixRMaj;

import java.util.ArrayList;
import java.util.List;

/**
 * Fuses information from multiple camera to create a single equirectangular image.  Each image
 * is rendered independently and added to the output image, but weighted by the mask.  The mask
 * describes the region of pixels in the equirectangular image which it represents.
 *
 * @author Peter Abeles
 */
public class MultiCameraToEquirectangular> {

	private EquirectangularTools_F32 tools = new EquirectangularTools_F32();
	private int equiWidth, equHeight;

	List cameras = new ArrayList<>();

	private T averageImage;
	private T workImage;
	private T cameraRendered;
	private GrayF32 weightImage;

	private ImageDistort distort;

	// how close two spherical coordinates need to be to be considered a match when doing back and forth validation
	// in radians
	private float maskToleranceAngle = UtilAngle.radian(0.1f);

	/**
	 * Configuration constructor
	 * @param distort Used to apply image distortion from different input images
	 * @param equiWidth Width of output equirectangular image
	 * @param equiHeight Height of output equirectangular image
	 * @param imageType Type of image it processes and outputs.  Must be floating point.  Hmm why isn't this fixed?
	 */
	public MultiCameraToEquirectangular(ImageDistort distort , int equiWidth , int equiHeight , ImageType imageType ) {

		if( imageType.getDataType().isInteger() || imageType.getDataType().getNumBits() != 32 )
			throw new IllegalArgumentException("Must be a 32 bit floating point image");

		this.distort = distort;
		this.equiWidth = equiWidth;
		this.equHeight = equiHeight;

		tools.configure(equiWidth, equiHeight);

		weightImage = new GrayF32(equiWidth,equiHeight);
		averageImage = imageType.createImage(equiWidth, equiHeight);
		workImage = averageImage.createSameShape();
		cameraRendered = averageImage.createSameShape();
	}

	/**
	 * Adds a camera and attempts to compute the mask from the provided distortion model.  if a pixel is rendered
	 * outside the bounds in the input image then it is masked out.  If the forwards/backwards transform is too
	 * different then it is masked out.
	 *
	 * @param cameraToCommon Rigid body transform from this camera to the common frame the equirectangular image
	 *                       is in
	 * @param factory Distortion model
	 * @param width Input image width
	 * @param height Input image height
	 */
	public void addCamera(Se3_F32 cameraToCommon , LensDistortionWideFOV factory , int width , int height ) {
		Point2Transform3_F32 p2s = factory.undistortPtoS_F32();
		Point3Transform2_F32 s2p = factory.distortStoP_F32();

		EquiToCamera equiToCamera = new EquiToCamera(cameraToCommon.getR(),s2p);

		GrayF32 equiMask = new GrayF32(equiWidth, equHeight);

		PixelTransform2_F32 transformEquiToCam = new PixelTransformCached_F32(equiWidth, equHeight,
				new PointToPixelTransform_F32(equiToCamera));

		Point3D_F32 p3b = new Point3D_F32();
		Point2D_F32 p2 = new Point2D_F32();

		for (int row = 0; row < equHeight; row++) {
			for (int col = 0; col < equiWidth; col++) {
				equiToCamera.compute(col,row,p2);

				int camX = (int)(p2.x+0.5f);
				int camY = (int)(p2.y+0.5f);

				if( Double.isNaN(p2.x) || Double.isNaN(p2.y) ||
						camX < 0 || camY < 0 || camX >= width || camY >= height )
					continue;

				p2s.compute(p2.x,p2.y,p3b);

				if( Double.isNaN(p3b.x) || Double.isNaN(p3b.y) || Double.isNaN(p3b.z))
					continue;

				double angle = UtilVector3D_F32.acute(equiToCamera.unitCam,p3b);

				if( angle < maskToleranceAngle) {
					equiMask.set(col,row,1);
				}
			}
		}
		cameras.add( new Camera(equiMask, transformEquiToCam));
	}

	/**
	 * Adds a camera and attempts to compute the mask from the provided distortion model.  if a pixel is rendered
	 * outside the bounds in the input image then it is masked out.  If the forwards/backwards transform is too
	 * different then it is masked out.
	 *
	 * @param cameraToCommon Rigid body transform from this camera to the common frame the equirectangular image
	 *                       is in
	 * @param factory Distortion model
	 * @param camMask Binary mask with invalid pixels marked as not zero.  Pixels are in camera image frame.
	 */
	public void addCamera(Se3_F32 cameraToCommon , LensDistortionWideFOV factory , GrayU8 camMask ) {

		Point2Transform3_F32 p2s = factory.undistortPtoS_F32();
		Point3Transform2_F32 s2p = factory.distortStoP_F32();

		EquiToCamera equiToCamera = new EquiToCamera(cameraToCommon.getR(),s2p);

		GrayF32 equiMask = new GrayF32(equiWidth, equHeight);

		PixelTransformCached_F32 transformEquiToCam = new PixelTransformCached_F32(equiWidth, equHeight,
				new PointToPixelTransform_F32(equiToCamera));

		int width = camMask.width;
		int height = camMask.height;

		Point3D_F32 p3b = new Point3D_F32();
		Point2D_F32 p2 = new Point2D_F32();
		for (int row = 0; row < equHeight; row++) {
			for (int col = 0; col < equiWidth; col++) {
				equiToCamera.compute(col,row,p2);

				if( UtilEjml.isUncountable(p2.x) || UtilEjml.isUncountable(p2.y) ) {
					// can't have it be an invalid number in the cache, but had to be invalid so that the mask
					// could be set to zero.  So set it to some valid value that won't cause it to blow up
					transformEquiToCam.getPixel(col,row).set(-1,-1);
					continue;
				}

				int camX = (int)(p2.x+0.5f);
				int camY = (int)(p2.y+0.5f);

				if( camX < 0 || camY < 0 || camX >= width || camY >= height )
					continue;

				if( camMask.unsafe_get(camX,camY) == 1 ) {
					p2s.compute(p2.x,p2.y,p3b);

					if( Double.isNaN(p3b.x) || Double.isNaN(p3b.y) || Double.isNaN(p3b.z))
						continue;

					double angle = UtilVector3D_F32.acute(equiToCamera.unitCam,p3b);

					if( angle < maskToleranceAngle) {
						equiMask.set(col,row,1);
					}
				}
			}
		}

		cameras.add( new Camera(equiMask, transformEquiToCam));
	}

	/**
	 * Provides recent images from all the cameras (should be time and lighting synchronized) and renders them
	 * into an equirectangular image.  The images must be in the same order that the cameras were added.
	 *
	 * @param cameraImages List of camera images
	 */
	public void render( List cameraImages ) {
		if( cameraImages.size() != cameras.size())
			throw new IllegalArgumentException("Input camera image count doesn't equal the expected number");

		// avoid divide by zero errors by initializing it to a small non-zero value
		GImageMiscOps.fill(weightImage,1e-4);
		GImageMiscOps.fill(averageImage,0);

		for (int i = 0; i < cameras.size(); i++) {
			Camera c = cameras.get(i);
			T cameraImage = cameraImages.get(i);

			distort.setModel(c.equiToCamera);
			distort.apply(cameraImage,cameraRendered);

			/// sum up the total weight for each pixel
			PixelMath.add(weightImage,c.mask,weightImage);

			// apply the weight for this image to the rendered image
			GPixelMath.multiply(c.mask,cameraRendered,workImage);

			// add the result to the average image
			GPixelMath.add(workImage, averageImage, averageImage);
		}

		// comput the final output by dividing
		GPixelMath.divide(averageImage,weightImage,averageImage);
	}

	public T getRenderedImage() {
		return averageImage;
	}


	/**
	 * Returns the mask for a specific camera
	 * @param which index of the camera
	 * @return Mask image.  pixel values from 0 to 1
	 */
	public GrayF32 getMask( int which ) {
		return cameras.get(which).mask;
	}

	public float getMaskToleranceAngle() {
		return maskToleranceAngle;
	}

	/**
	 * Specify the tolerance that the circle normal angle must be invertible in radians
	 *
	 * @param maskToleranceAngle tolerance in radians
	 */
	public void setMaskToleranceAngle(float maskToleranceAngle) {
		this.maskToleranceAngle = maskToleranceAngle;
	}

	static class Camera {
		// weighted pixel mask in equi image.  0 = ignore pixel.  1 = 100% contribution
		GrayF32 mask;

		PixelTransform2_F32 equiToCamera;

		public Camera(GrayF32 mask, PixelTransform2_F32 equiToCamera) {
			this.mask = mask;
			this.equiToCamera = equiToCamera;
		}
	}

	/**
	 * Transform from equirectangular image to camera image pixels
	 */
	private class EquiToCamera implements Point2Transform2_F32 {

		FMatrixRMaj cameraToCommon;
		Point3Transform2_F32 s2p;

		Point3D_F32 unitCam = new Point3D_F32();
		Point3D_F32 unitCommon = new Point3D_F32();

		EquiToCamera(FMatrixRMaj cameraToCommon, Point3Transform2_F32 s2p) {
			this.cameraToCommon = cameraToCommon;
			this.s2p = s2p;
		}

		@Override
		public void compute(float x, float y, Point2D_F32 out) {
			// go from equirectangular pixel to unit sphere in common frame
			tools.equiToNormFV(x,y, unitCommon);

			// rotate the point into camera frame
			GeometryMath_F32.multTran(cameraToCommon, unitCommon, unitCam);

			// input camera image pixels
			s2p.compute(unitCam.x, unitCam.y, unitCam.z , out);
		}
	}
}