boofcv.gui.d3.DisparityPointCloudViewer Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of visualize Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
The newest version!
/*
 * 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.gui.d3;

import boofcv.misc.BoofMiscOps;
import boofcv.struct.distort.Point2Transform2_F64;
import boofcv.struct.image.GrayF32;
import boofcv.struct.image.GrayU8;
import boofcv.struct.image.ImageBase;
import boofcv.struct.image.ImageGray;
import georegression.geometry.ConvertRotation3D_F64;
import georegression.geometry.GeometryMath_F64;
import georegression.struct.EulerType;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Point3D_F64;
import georegression.struct.point.Vector3D_F64;
import georegression.struct.se.Se3_F64;
import georegression.transform.se.SePointOps_F64;
import org.ddogleg.struct.FastQueue;
import org.ejml.data.DenseMatrix64F;

import javax.swing.*;
import java.awt.*;
import java.awt.image.BufferedImage;

/**
 * 
 * Renders a 3D point cloud using a perspective pin hole camera model.
 * 
 *
 * 
 * Rendering speed is improved by first rendering onto a grid and only accepting the highest
 * (closest to viewing camera) point as being visible.
 * 
 *
 * @author Peter Abeles
 */
public class DisparityPointCloudViewer extends JPanel {
	FastQueue cloud = new FastQueue<>(200, ColorPoint3D.class, true);

	// distance between the two camera centers
	double baseline;

	// intrinsic camera parameters
	DenseMatrix64F K;
	double focalLengthX;
	double focalLengthY;
	double centerX;
	double centerY;

	// minimum disparity
	int minDisparity;
	// maximum minus minimum disparity
	int rangeDisparity;

	// How far out it should zoom.
	double range = 1;

	// view offset
	double offsetX;
	double offsetY;

	// Data structure that contains the visible point at each pixel
	// size = width*height, row major format
	Pixel data[] = new Pixel[0];

	// tilt angle in degrees
	public int tiltAngle = 0;
	public double radius = 5;

	// converts from rectified pixels into color image pixels
	Point2Transform2_F64 rectifiedToColor;
	// storage for color image coordinate
	Point2D_F64 colorPt = new Point2D_F64();

	/**
	 * Stereo and intrinsic camera parameters
	 * @param baseline Stereo baseline (world units)
	 * @param K Intrinsic camera calibration matrix of rectified camera
	 * @param rectifiedToColor Transform from rectified pixels to the color image pixels.
	 * @param minDisparity Minimum disparity that's computed (pixels)
	 * @param maxDisparity Maximum disparity that's computed (pixels)
	 */
	public void configure(double baseline,
						  DenseMatrix64F K,
						  Point2Transform2_F64 rectifiedToColor,
						  int minDisparity, int maxDisparity) {
		this.K = K;
		this.rectifiedToColor = rectifiedToColor;
		this.baseline = baseline;
		this.focalLengthX = K.get(0,0);
		this.focalLengthY = K.get(1,1);
		this.centerX = K.get(0,2);
		this.centerY = K.get(1,2);
		this.minDisparity = minDisparity;

		this.rangeDisparity = maxDisparity-minDisparity;


	}

	/**
	 * Given the disparity image compute the 3D location of valid points and save pixel colors
	 * at that point
	 *
	 * @param disparity Disparity image
	 * @param color Color image of left camera
	 */
	public void process(ImageGray disparity , BufferedImage color ) {
		if( disparity instanceof GrayU8)
			process((GrayU8)disparity,color);
		else
			process((GrayF32)disparity,color);
	}

	private void process(GrayU8 disparity , BufferedImage color ) {

		cloud.reset();

		for( int y = 0; y < disparity.height; y++ ) {
			int index = disparity.startIndex + disparity.stride*y;

			for( int x = 0; x < disparity.width; x++ ) {
				int value = disparity.data[index++] & 0xFF;

				if( value >= rangeDisparity )
					continue;

				value += minDisparity;

				if( value == 0 )
					continue;

				ColorPoint3D p = cloud.grow();

				// Note that this will be in the rectified left camera's reference frame.
				// An additional rotation is needed to put it into the original left camera frame.
				p.z = baseline*focalLengthX/value;
				p.x = p.z*(x - centerX)/focalLengthX;
				p.y = p.z*(y - centerY)/focalLengthY;

				getColor(disparity, color, x, y, p);
			}
		}
	}

	private void process(GrayF32 disparity , BufferedImage color ) {

		cloud.reset();

		for( int y = 0; y < disparity.height; y++ ) {
			int index = disparity.startIndex + disparity.stride*y;

			for( int x = 0; x < disparity.width; x++ ) {
				float value = disparity.data[index++];

				if( value >= rangeDisparity )
					continue;

				value += minDisparity;

				if( value == 0 )
					continue;

				ColorPoint3D p = cloud.grow();

				p.z = baseline*focalLengthX/value;
				p.x = p.z*(x - centerX)/focalLengthX;
				p.y = p.z*(y - centerY)/focalLengthY;

				getColor(disparity, color, x, y, p);
			}
		}
	}

	private void getColor(ImageBase disparity, BufferedImage color, int x, int y, ColorPoint3D p) {
		rectifiedToColor.compute(x,y,colorPt);
		if( BoofMiscOps.checkInside(disparity, colorPt.x, colorPt.y, 0) ) {
			p.rgb = color.getRGB((int)colorPt.x,(int)colorPt.y);
		} else {
			p.rgb = 0x000000;
		}
	}

	@Override
	public synchronized void paintComponent(Graphics g) {
		super.paintComponent(g);

		projectScene();

		int width = getWidth();
		int h = getHeight();

		int r = 2;
		int w = r*2+1;

		Graphics2D g2 = (Graphics2D)g;

		int index = 0;
		for( int y = 0; y < h; y++ ) {
			for( int x = 0; x < width; x++ ) {
				Pixel p = data[index++];
				if( p.rgb == -1 )
					continue;

				g2.setColor(new Color(p.rgb));
				g2.fillRect(x - r, y - r, w, w);
			}
		}
	}

	private void projectScene() {
		int w = getWidth();
		int h = getHeight();

		int N = w*h;

		if( data.length < N ) {
			data = new Pixel[ N ];
			for( int i = 0; i < N; i++ )
				data[i] = new Pixel();
		} else {
			for( int i = 0; i < N; i++ )
				data[i].reset();
		}

		Se3_F64 pose = createWorldToCamera();
		Point3D_F64 cameraPt = new Point3D_F64();
		Point2D_F64 pixel = new Point2D_F64();

		for( int i = 0; i < cloud.size(); i++ ) {
			ColorPoint3D p = cloud.get(i);

			SePointOps_F64.transform(pose,p,cameraPt);
			pixel.x = cameraPt.x/cameraPt.z;
			pixel.y = cameraPt.y/cameraPt.z;

			GeometryMath_F64.mult(K,pixel,pixel);

			int x = (int)pixel.x;
			int y = (int)pixel.y;

			if( x < 0 || y < 0 || x >= w || y >= h )
				continue;

			Pixel d = data[y*w+x];
			if( d.height > cameraPt.z ) {
				d.height = cameraPt.z;
				d.rgb = p.rgb;
			}
		}
	}

	public Se3_F64 createWorldToCamera() {
		// pick an appropriate amount of motion for the scene
		double z = baseline*focalLengthX/(minDisparity+rangeDisparity);

		double adjust = baseline/20.0;

		Vector3D_F64 rotPt = new Vector3D_F64(offsetX*adjust,offsetY*adjust,z* range);

		double radians = tiltAngle*Math.PI/180.0;
		DenseMatrix64F R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ,radians,0,0,null);

		Se3_F64 a = new Se3_F64(R,rotPt);

		return a;
	}

	/**
	 * Contains information on visible pixels
	 */
	private static class Pixel
	{
		// the pixel's height.  used to see if it is closer to the  camera or not
		public double height;
		// Color of the pixel
		public int rgb;

		private Pixel() {
			reset();
		}

		public void reset() {
			height = Double.MAX_VALUE;
			rgb = -1;
		}
	}
}