boofcv.alg.distort.spherical.MultiCameraToEquirectangular Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of geo Show documentation
Show all versions of geo Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
/*
* Copyright (c) 2011-2016, 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_F64;
import org.ejml.data.DenseMatrix64F;
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_F64 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_F64 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);
PixelTransform2_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);
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;
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 {
DenseMatrix64F cameraToCommon;
Point3Transform2_F32 s2p;
Point3D_F32 unitCam = new Point3D_F32();
Point3D_F32 unitCommon = new Point3D_F32();
EquiToCamera(DenseMatrix64F 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);
}
}
}