• Home
• org.boofcv
• boofcv-ip-multiview
• 1.1.4
• source code
• MultiCameraToEquirectangular.java

×

Project download

Many resources are needed to download a project. Please understand that we have to compensate our server costs. Thank you in advance.
Project price only 1 $

You can buy this project and download/modify it how often you want.

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

/*
 * 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.distort;

import boofcv.alg.distort.spherical.EquirectangularTools_F32;
import boofcv.alg.misc.GImageMiscOps;
import boofcv.alg.misc.GPixelMath;
import boofcv.alg.misc.PixelMath;
import boofcv.struct.distort.*;
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 final EquirectangularTools_F32 tools = new EquirectangularTools_F32();
	private final int equiWidth;
	private final int equHeight;

	List cameras = new ArrayList<>();

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

	private final 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);

		PixelTransform 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).setTo(-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;

		PixelTransform equiToCamera;

		public Camera( GrayF32 mask, PixelTransform 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);
		}

		@Override public EquiToCamera copyConcurrent() {
			throw new RuntimeException("Implement");
		}
	}
}